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ABSTRACT 



This Software Reference documents and summarizes all source code produced for a 
Ph D. dissertation constructing an underwater virtual world for an Autonomous Underwater 
Vehicle (AUV). 

A critical bottleneck exists in Autonomous Underwater Vehicle (AUV) design and 
development. It is tremendously difficult to observe, communicate with and test underwater 
robots, because they operate in a remote and hazardous environment where 
physical dynamics and sensing modalities are counterintuitive. 

An underwater virtual world can comprehensively model all salient functional 
characteristics of the real world in real time. This virtual world is designed from the 
perspective of the robot, enabling realistic AUV evaluation and testing in the laboratory. 
Three-dimensional real-time computer graphics are our window into that virtual world. 

Visualization of robot interactions within a virtual world permits sophisticated analyses 
of robot performance that are otherwise unavailable. Sonar visualization permits researchers 
to accurately "look over the robot’s shoulder” or even "see through the robot’s eyes” to 
intuitively understand sensor-environment interactions. Extending the theoretical derivation 
of a set of six-degree-of-freedom hydrodynamics equations has provided a fully general 
physics-based model capable of producing highly non-linear yet experimentally-verifiable 
response in real time. 

Distribution of underwater virtual world components enables scalability and real-time 
response. The IEEE Distributed Interactive Simulation (D1S) protocol is used for compatible 
live interaction with other virtual worlds. Network connections allow remote access, 
demonstrated via Multicast Backbone (MBone) audio and video collaboration with 
researchers at remote locations. Integrating the World-Wide Web allows rapid access to 
resources distributed across the Internet. 

This dissertation presents the frontier of 3D real-time graphics to support underwater 
robotics, scientific ocean exploration, sonar visualization and worldwide collaboration. 
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I. INTRODUCTION 

The purpose of this guide is to document the source code developed in the 
creation of an underwater virtual world for an autonomous underwater vehicle (AUV). 
It is companion document to a dissertation (Brutzman 94). There are a number of 
parts to this work. First a users guide is presented which describes how to obtain and 
install the software described here. Key mission output files then show results 
produced by the autonomous underwater robot interacting with the virtual world. 
Remaining chapters present the source code developed for robot execution software, 

3D computer graphics, six degree-of-freedom hydrodynamics, a geometric sonar 
model, networking, and hypermedia using the World-Wide Web and the Multicast 
Backbone (MBone). 

Several languages are included in this source code. Robot execution programs 
and supporting configuration files are written in the Kemigan and Richie variant of the 
C language, and run identically under Irix 5.2 or OS-9 operating systems. Other 
source programs are written primarily in C++. Source code file name conventions use 
a .c extension for C language files, and a .C extension for C++ language files. 
Remaining programs and files are written in languages specific to the application, such 
as xnuplot (Williams 95), Tool control language {Tel) (Osterhout 94) (Welch 94), 
hypertext markup language (.html) (Berners-Lee 94a, 94b) (Hughes 94) or operating 
system shell scripts. 

All of the programs described here are available electronically without charge to 
Internet users via anonymous ftp. The accompanying public electronic distribution of 
this reference includes both source code and compiled executable programs. All 
programs have been tested under SGI operating system Irix 5.2, and robot code has 
also been tested under operating system OS-9 version 2.3. Goals of the underwater 
virtual world project include making source code, applications and interfaces easily 
available in order to encourage scientific collaboration and educational use. Future 



work includes porting virtual world viewers to a variety of operating systems and 
computer architectures. 

An NPS AUV hypermedia home page has been prepared to facilitate 
examination and download of the various underwater virtual world components. This 
home page is available for interaction using any number of Internet-based browsers, 
most notably Mosaic (NCSA 94a). Resources on the home page include the electronic 
distribution, installation and execution instructions, pointers to related work, images, 
plots, papers and a variety of other media. World-Wide Web (WWW) Uniform 
Resource Locator (URL) for the NPS AUV home page is: 
ftp:lltaunis.cs.nps.mvy.millpublauvlauv.html 

To learn more about the World-Wide Web, consult "Entering the World-Wide 
Web (WWW): A Guide to Cyberspace" (Hughes 94). This paper describes everything 
needed to start using Mosaic , accessing all types of information sources, and using 
hypermedia and the Internet. Mosaic is a hypermedia browser that is freely available 
for Unix machines running X windows. Macintoshes and personal computers (PCs) 
running Windows. There are also free line mode browsers (such as lynx and www) 
that let you browse hypertext documents in a simple text mode using any computer 
which has an internet connection (Montulli 94) (Berners-Lee 94b). 

The source code in this software guide is fully documented and reasonably 
self-explanatory. Graphics and hydrodynamics programs are written in the ANSI 
standard C++ language, robot execution code and networking code is written in the 
Kernighan and Richie variant of the C language, plotting programs use 
gnuplot version 3.5 scripting language, and hypertext pages are written in standard 
Hypertext Markup Language (. html ) (NCSA 94b). Despite extensive documentation, 
however, one programmer’s clearly-written code may well be undecipherable to 
another programmer. Users are strongly encouraged to review the companion 
dissertation which derives the formulae and explains the concepts behind the software 
(Brutzman 94), retrieve the latest version of the underwater virtual world distribution, 
or contact the author for questions that remain unanswered. No guarantees of any 



i 



kind are implied. Many problems are open and progress occurs on a frequent basis 
when conducting research on robots and virtual worlds. 

Hopefully this work will stimulate and encourage other research efforts to build 
large-scale virtual worlds in compatibly-extendable ways. Interested readers are 
encouraged to examine the AUV home page for project updates, read the 
documentation, and to contact the author regarding new uses of the software and 
potential research collaboration. 
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II. INSTALLATION, EXECUTION, CREATION ANI) REMOVAL 



A. Introduction 

The following Installation and Execution Guide is intended to enable any new 
user to download, install, execute and understand the NPS AUV Underwater Virtual 
World. Users may want to solely download the viewer software to observe an 
Internet- wide mission exercise, or run the entire set of programs making up the virtual 
world. Users of the robot execution program can enter their electronic mail address 
and receive a mission report following each mission. A sample mission report shows 
the mission script used for the canonical SIGGRAPH 94 mission, a project abstract, 
pointers to related work and the execution orders generated by the mission script. 
Mission reports are also available by "fingering" the home auv account, e.g. 

finger auv(a ducle.cs.nps.navy.mil 

Finally, shell script files for archive creation and removal are included to 
document automated archive maintenance. 
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B. auv-uvw. IN STALL Installation and Execution Guide 



// 



// 



Installation & Execution Guide for the NPS AUV Underwater Virtual World 
auv-uvw. INSTALL 19 OCT 94 
Don Brutzman brutzmandnps.navy.mil 
URL: ftp : //taurus . cs .nps .navy.mil/pub/auv/auv-uvw.virtual-world . INSTALL 



// // 

Table of Contents 

Distribution information 
System requirements 

Retrieving the distribution auv-uvw . tar . Z 
Installing the distribution auv-uvw . tar . Z 
MBone connection (optional but recommended) 

Information files 

Viewing remotely-executing robot missions via the MBone 
Running all of the virtual world components locally 
Plotting mission telemetry using gnuplot 
Recompiling virtual world component executables 
Killing multicast processes 
Removing the entire distribution 
Future work 
Contact information 

This is a big project! I've tried to make this guide as concise and readable 



as possible. However some work is necessary to read and absorb this material. 
Suggestions on how to further streamline the guide are very welcome. Thanks! 

// // 



Distribution information 

For the latest greatest distribution, plus pointers to all of the supporting 
public domain programs, use Mosaic or a World-Wide Web line browser to 
connect to the NPS AUV home page : 

ftp : //taurus . cs .nps .navy.mil/pub/auv/auv.html 

To retrieve the distribution directly: 

ftp : //taurus . cs . nps . navy . mil/pub/auv/auv-uvw . tar . Z 

To receive a copy of a recent mission report: 

finger auvddude.cs.nps.navy.mil 



// // 

System requirements 

The auv-uvw 'viewer' requires SGI Irix 5.2 or better with Openlnventor 2.0 
Execution Only Environment ( invent or_eoe ) installed. This is provided 
free with all versions of Irix, it merely needs to be installed. 

You can tell if it is installed by typing 'versions' at the SGI prompt. 

Modification and recompilation of the 'viewer' requires installation of 
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inventor_dev Inventor 3D Toolkit, 2.0 development option. 

The virtual world 'viewer' is written in C++ using Openlnventor 
graphics, and it also uses the NPSNET DIS 2.0.3 multicast libraries 
(which are also included as source/binaries in this distribution) . 

The 'dynamics' virtual world software has been tested under Irix 5.2, 
is written in C++, and also uses the NPSNET DIS 2.0.3 multicast 
libraries. You should run it on a sound-equipped workstation for 
best results . 

The robot 'execution' code has been tested under Irix 5.2 and OS-9 v2.3 
It is written in ANSI C with special #defines for OS-9 KfitR C 
compatibility, allowing it to be run without modifications either under 
Irix or on the NPS Autonomous Underwater Vehicle 68030 microprocessor. 
It ought to be portable to most any plain vanilla C compiler. 



// - - 

Retrieving the distribution auv-uvw . tar . Z 

The easiest way is to retrieve & save the tar file is to point Mosaic at 

ftp : //taurus . cs . nps . navy.mil/pub/mosaic/auv . html 

If instead you use anonymous ftp, here is a sample session: 

unix>> ftp taurus.cs.nps.navy.mil 
Connected to taurus.cs.nps.navy.mil. 

220 taurus FTP server (Version 6.10 Wed Mar 18 11:57:03 PST 1992) ready. 

Name (taurus . cs . nps . navy . mil : yourname ) : anonymous 
331 Guest login ok, send e-mail address as password. 

Password : your . email . address . here 

ftp> cd pub/auv 

250 CWD command successful. 

ftp> binary 

200 Type set to I. 

ftp> get auv-uvw . tar . Z 

local: auv-uvw . tar . Z remote: auv-uvw . tar . Z 

150 Opening BINARY mode data connection for auv-uvw .tar . Z (4747669 bytes) 
226 Transfer complete. 

4747669 bytes received in 34.32 seconds (135.08 Kbytes/s) 
ftp> quit 

221 Goodbye. 

The uncompressed distribution is about 10 MB, not counting the extra 
recommended applications. The compressed distribution is about 5 MB. 
Precise distribution sizes can vary when changes are incorporated. 



// — 

Installing the distribution auv-uvw . tar . Z 

Uncompress auv-uvw . tar . Z in your root directory by typing 
unix: yourhome> uncompress auv-uvw . tar . Z 

unix:yourhome> tar -xvf auv-uvw. tar 



Backup fc update your MBone session directory configuration file .sd.tcl 



unix : yourhome > 



cp .sd.tcl 



. sd . tel . old 



Unix : yourhome > 



cp .sd.tcl.auv-uvw .sd.tcl 



Edit the MBone tool aliases to include directory paths as appropriate. 

They are located at the very end of the new file .sd.tcl 

Unix : yourhome > zip .sd.tcl 

After editing, exit and restart session directory (sd) : 

Unix: yourhome > sd 

You can now run the viewer from an sd advertisment for the NPS AUV Underwater 
Virtual World (if there is one running - this is optional) . 



You will notice some auv-uvw information files in your root directory 
and several new directories: 

yourhome/ a uv-uvw/ Openlnventor viewer program & data files 

yourhome /exe cut ion AUV (robot) execution level code and 
mission scripts 

yourhome/dynamics/Virtual world for robot: dynamics, sonar 
and other components 

yourhome /dynamics/speecJText -to-speech audio files from mission 
scripts. These originate from queries 
to the Say. . server 



The auv-uvw distribution should now be fully installed. 



// - - -// 

MBone connection (optional but recommended) 

Multicast Backbone (MBone) connectivity is needed if you want 
to participate in remote exercises. A good way to test your network's 
connectivity is to type 'sd' (session directory) . If you get a menu 
of MBone programs starting to slowly build up, you are in business. 

To receive Distributed Interactive Simulation (DIS) robot execution 
Protocol Data Units (PDUs) originating on your own network, you 
do not have to be connected to the Internet- wide MBone. 



SGI ' s Irix 5.2 operating system supports multicast in the kernel, so 
you can still run the robot /virtual world programs on one machine fc 
the auv-uvw viewer on another. Only one process per machine can grab 
the multicast port, thus at least 2 machines are needed to run 
everything . 

To connect your system to the MBone takes some work but is worth it. 
You will need the involvement & support of your network administrator. 



The following article tells why MBone is so great and how to connect: 

ftp://taurus.cs.nps.navy.mil/pub/mbmg/mbone.ps (PostScript) or 
ftp://taurus.cs.nps.navy.mil/pub/mbmg/mbone.html (hypertext) or 
ftp://taurus.cs.nps.navy.mil/pub/mbmg/mbone.txt (ASCII text) 



Other installation pointers are on the NPS MBone home page at 
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ftp : //taurus . cs . nps . navy.mil/pub/mosaic/mbone . html 

Over a thousand subnets worldwide are already connected to the MBone 
There are lots of people willing to help. In practice we've seen it 
take people between a few days and a few weeks to start using MBone # 
but once connected further work is rarely required. Good luck! 



// - ----// 

Information files: 

auv-uvw .virtual -world . README (a mission report) 

auv-uvw. virtual -world. INSTALL (this file) 

others are listed in the mission report and the home page site 



// - // 

Viewing remotely-executing robot missions via the MBone 

You must have MBone connectivity installed on your network for remote 
experiment observation. If the NPS AUV Underwater Virtual World 
is advertised in sd, and you have installed the distribution, 
just select the session and multicast ports are passed automatically 
to the viewer. 

Alternatively, you can start the viewer manually from the command line: 

unix : yourhomo auv-uvw . viewer 

or unix:yourhome/auv-uvwviewer 

The following command line switches are available with viewer: 

-address 224 . 2 . ### . ##nulticast address as advertised in sd session 
-port ##### multicast wb port as advertised in sd session 

The virtual world viewer should now run. 



// // 

Running all of the virtual world components locally 

Running a robot mission takes three processes and at least two machines, 
viewer and dynamics must be on different hosts, execution can run anywhere. 

Notes regarding sd session advertisements are optional. 

* First is the 'viewer' to get a window into the virtual world. Run this 
on your most capable Iris workstation - Reality Engines are nice! 

You can use or disable texturing based on the capabilities of the machine. 

unixl> cd auv-uvw 
unixl> viewer 

The following optional command line switches are available with viewer: 

-address 224 . 2 . ### . ##nulticast address as advertised in sd session 
-port ##### multicast wb port as advertised in sd session 

-texture-on 
-texture -off 

-printdialog pop up print dialog box for screen snapshots 
-noprint dialog 

The virtual world viewer now runs . 
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* The 'dynamics' program is the virtual world connection for robot execution: 
Run it on a sound -equipped workstation for best results, otherwise ignore any 
sound-card-missing error messages. The 'dynamics' program is purely 
text-based so graphics capabilities are irrelevant. 

unix2> cd dynamics 
unix2> dynamics 

Dynamics classes test selections 

L loop_test_with_execution_level (); 

M Multicast parameter input 

tt 1=15 , group address=224 . 2 . 121 . 93 , port=3111 

0 Ocean current vector reset <0, 0, 0> 

H Hmatrix/quatemion exerciser 

R Rotation of quaternion & Hmatrix using p q r 
D Defaults 

1 Invert matrix test 

E dEad__reckon_test_with_execution_level 
P PDU_skip_interval change (from 1) 

T Toggle TRACE = 0 
C DIS_net_close (); 

Q Quit 

Enter choice : 

Select L for loop test with execution level 

The following command line switches are available with dynamics program: 

-ttl # time to live (be careful!) recommend 16 or less 

-address 224 . 2 . ### . ##nulticast address as advertised in sd session 
-port ##### multicast wb port as advertised in sd session 

-loop start looping automatically 

* Finally, 'execution' is the robot execution level software. 

'execution' runs through the file ' mission . script ' and gets 
real world responses from the virtual world program 'dynamics'. 

'execution' can run under Unix or real-time operating system OS-9. 

unix3> cd execution 

unix3> cp mission . script . the_one_you_want mission . script 

unix3> execution remotehost unixl . host . name 

Enter your e-mail in the execution process window when asked. 

See the file mission . script . HELP for info on how to write mission scripts. 



// // 

Plotting mission telemetry using gnuplot 
unix3 : yourhome> cd execution 

unix3 : yourhome/ execution /^gnup lot auv_plot .gnu 
For fewer points plotted (1 per second) out of the same dataset: 
unix3 : yourhome/execution/xjnuplot auv_plot_l_second . gnu 



or use a postscript viewer (xpsview or ghostview) to look at plots directly: 

unix3> xpsview -wp -skipc -or landscape -/execut ion/AUV_telemetry .ps & 
or unix3> ghostview -landscape -/execut ion/AUV_telemetry . ps & 



// 



// 
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Recompiling virtual world component executables 
auv-uvw directory 

(Requires inventor_dev Inventor 2.0 development option installed. 
See system requirements section for details) . 

unix3 : yourhome/ auv-uvw/ > make viewer 



dynamics directory 

unix3 : yourhome /dynamics /> make dynamics 



execution directory 

unix3 : yourhome /execution/ > copy Makefile. SGI Makefile 
unix3 : yourhome /exe cut ion/ > make execution 

or OS- 9: yourhome/execution/> copy Makefile. OS9 Makefile 
OS-9: yourhome /execution/ > make execution 



Details on recompilation and modification are included in all source files. 



// -// 

Killing multicast processes 

Sometimes multicast processes (such as 'viewer' and 'dynamics') do not die 
cleanly because they have forked a second multicast process, 
viewer is especially prone to this behavior when invoked from session 
directory (sd) , since sd must put the viewer process into the background. 

You may need to take special action to ensure the processes are killed. 

The following instructions will work on SGI machines: 

To put a process to sleep that refuses to let go of the command prompt: 
type a (control-Z) in the runaway process window 
To see what processes are running: 
unix> ps 

To see ALL processes that are running (including previous zombie processes) : 
unix> ps -ea 
To kill a process: 

unix> kill -9 #### where #### is the Process ID (PID) 



An example log: 

[runaway process, you hit the control-Z key to suspend it] 
A Z 

Suspended 
unix>> ps 



PID 


TTY 


TIME 


COMD 


16558 


ttyq3 


9:37 


viewer 


11289 


ttyq3 


0 : 01 


tcsh 


16581 


ttyq3 


0:00 


viewer 


16902 


ttyq3 


0 : 0 0 


ps 
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unix>> kill -9 16558 16581 



unix>> ps 

PID TTY 
11289 ttyq3 
16904 ttyq3 



TIME COMD 
0 : 01 tcsh 
0:00 p s 



n--- - // 

Removing the entire distribution: 

Be sure to move any files that you want to save to different directories. 

You do not have to remove the old distribution prior to updating 
to a new version. However you might want to, so that superceded files 
in old distributions are not left over. 

To remove everything, run the shell script auv-uvw . virtual -world . REMOVE : 
unix> source auv-uvw .virtual -world . REMOVE 



// // 

Future work (lots!) 

Porting the Openlnventor viewer code to other architectures and 
porting other virtual world components to other architectures 
depends on availability of Openlnventor and multicast TCP/IP support. 
Porting is planned for 1995. 

Other people are always welcome to use and extend this code. 

Please keep me informed of your efforts if you find it of value. 

Other research collaborations to extend the underwater virtual world 
to build up a truly large-scale Interne t -wide fully distributed 
virtual world are of particular interest. 

Integrate Dr. Larry Ziomek's Recursive Ray Acoustics (RRA) sonar model. 

Incorporate pre-processed terrain datasets for Monterey Bay. 

Virtual Reality Modeling Language (VRML) - see the working group 
http : //www . wired . com/vrml/ 

Dissertation "A Virtual World for an Autonomous Underwater Vehicle" 
is being written and will be made publicly available . 



// // 

Contact information: 

Don Brutzman brut zman®nps . navy . mil 

Code OR/Br Naval Postgraduate School [Glasgow 204] work (408) 656-2149 

Monterey California 93943-5000 USA fax (408) 656-2595 



// 



// 



li 



C. mission. output, email Mission Output Electronic Mail Report 



The following report is sent by electronic mail to users who execute a robot 
mission. It demonstrates that robots, once networked, can report back results only 
when they are of interest or human intervention is specifically desired. The goal is 
for the robot to meet user directives conveniently, rather than requiring constant 
monitoring or supervision. 

The mission report combines an NPS AUV reference information file, the 
mission script that drove the robot, and the resulting time line of propulsor and plane 
surface orders. It is also available by querying the robot account with the Unix 
"finger" command: finger auv@cs.dude.nps.navy.mil 



NPS AUV Mission Report 

Here is a mission report generated by the Naval Postgraduate School (NPS) 
Autonomous Underwater Vehicle (AUV) running in an Underwater Virtual World. 

This message was created by the operating robot connected to the network. 
Actual robots with network connections can send reports such as this 
to research teams and interested individuals when scientific discoveries 
are made or unusual circumstances occur. 



virtually yours, the NPS AUV. 



8-/ nps auv ) 
8 -\ ) 



The following Universal Resource Locators (URL's) are pointers that lead to 
more information about the NPS AUV Underwater Virtual World & related work. 

* The AUV underwater virtual world source code, information files and 

* executable programs (for SGI Irix 5.2 machines) can be used to monitor 

* NPS AUV DIS PDU's from anywhere on the Internet MBone . It is freely 

* available via anonymous ftp as a compressed tar file at 
ftp : //taurus . cs .nps .navy.mil/pub/auv/auv_uvw.tar . Z 

* NPS AUV home page free software distribution, research summary and 

* anonymous ftp directory: 

ftp -.//taurus .cs .nps .navy.mil/pub/mosaic/auv.html 
ftp : //taurus . cs . nps .navy.mil/pub/auv/AUVPAPERS 
ftp : //taurus . cs .nps .navy.mil/pub/auv/ 

* Naval Postgraduate School World-Wide-Web (WWW) home page includes many 

* additional pointers to the NPSNET Virtual Battlefield, DIS, MBone, and 

* I3LA regional research around Monterey Bay; 

ftp : //taurus . cs .nps .navy.mil/pub/mosaic/nps_mosaic.html 

* To learn about Internet audio & video on the Multicast Backbone (MBONE) : 
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ftp : //taurus . cs .nps . navy.mil/pub/mosaic/mbone . html 

* How the robot voice was synthesized live from text over the Internet using 

* Axel Belinfante's "Say..." speech server at University of Twente, 

* Netherlands which uses Nick Ing-Simmons' phoneme synthesizer 'rsynth': 
http: //utis!79 . cs .utwente . nl : 6001/say/? 



* Postscript plots (20 pages) of vehicle telemetry during SIGGRAPH mission: 
ftp://taurus .cs .nps .navy . mil/pub/auv/SIGGRAPH94/AUV_telemetry . ps .2 

* The people from NPS involved in SIGGRAPH 94 _The Edge_ NPS AUV exhibit: 
ftp : //taurus . cs .nps .navy.mil/pub/pratts/home .html 



* Learning more about the World-Wide-Web, URLs and Mosa 

* "Entering the World-Wide Web: A Guide to Cyberspace" 
http : //www . eit . com/web/www . guide/ 

ftp://ftp.eit. com/pub/ web .guide /guide . 61 /guide . 61 . ps . Z 
ftp://ftp.eit. com/pub/ web .guide /guide . 61 /guide . 61 . txt 



ic : 

by Kevin Hughes 
hypertext 
Postscript 
text 



* A recent copy of this mission report can be found by an auv account finger: 
finger auv®dude . cs .nps . navy .mil 



If you can't find taurus.cs.nps.navy.mil, it has IP number 131.120.1.13 
Additional references are available on request. 



This project is part of a PhD dissertation. Here is the abstract: 

A Virtual World for an Autonomous Underwater Vehicle 
Don Brutzman 

Code OR/Br, Naval Postgraduate School 
Monterey California 93943-5000 USA 
(408) 656-2149 office, (408) 656-2595 fax 

brutzman@nps . navy . mil 

A critical bottleneck exists in Autonomous Underwater Vehicle (AUV) 
design and development. It is tremendously difficult to observe, communicate 
with and test underwater robots, because they operate in a remote and 
hazardous environment where physical dynamics and sensing modalities are 
counterintuitive . 

An underwater virtual world can comprehensively model all salient 
functional characteristics of the real world in real time. This virtual 
world is designed from the perspective of the robot, enabling realistic AUV 
evaluation and testing in the laboratory. 3D real-time graphics are our 
window into that virtual world. Visualization of robot interactions within 
a virtual world permits sophisticated analyses of robot performance that 
are otherwise unavailable. Sonar visualization permits researchers to 
accurately "look over the robot's shoulder" or even "see through the 
robot's eyes" to intuitively understand sensor-environment interactions. 

Distribution of underwater virtual world components enables scalability 
and real-time response. The IEEE Distributed Interactive Simulation (DIS) 
protocol is used for compatible live interaction with other virtual worlds. 
Network access allows individuals remote access . This is demonstrated via 
MBONE collaboration with others outside The Edge, and Mosaic access to 
pertinent archived images, papers, datasets, software, sound clips, text 
and any other computer-storable media. 

This project presents the frontier of 3D real-time graphics for 
underwater robotics, ocean exploration, sonar visualization and worldwide 
scientific collaboration. 






Questions, comments and collaborations are welcome. Please contact : 
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Don Brutzman brut zman@nps . navy . mi 1 

Code OR/Br Naval Postgraduate School [Glasgow 204) work (408) 656-2149 

Monterey California 93943-5000 USA fax (408) 656-2595 

SIGGRAPH 94 Collaborators: Michael J. Zyda , Paul T. Barham, John S. Falby, 

Anthony J. Healey, Shirley Isakari, Rodney Luck, 
Michael R. Macedonia, Robert B. Mcghee, 

David R. Pratt, Lawrence J. Ziomek 

Naval Postgraduate School, Monterey California 



Your AUV robot mission completed successfully. Here is the mission you ran: 

# your mission is 

# mission . script . rotate_test 

time 0 

timestep 0.1 

# initialize vehicle 
depth 45 

position 0 0 45 

orientation 000 

thrusters-on 

rotate 16 

pause 

wait 60 

rotate 0 

thrusters -of f 

wait 120 

step 

keyboard 

keyboard-off 

# mission complete 
kill 

# NPS AUV file mission . output . orders : commanded propulsion orders versus time 

# 

# timestep: 0.10 seconds 

# 



# 


time 


heading 


North 


East 


Depth 


rpm 


rpm 


stern 


stern 


vertical 


lateral 


# 




X 


y 


z 


port 


stbd 


plane 


rudder 


thrusters 


thrusters 


# 




















bow/ stern 


bow/stern 




0.0 


0.0 


0.0 


0 . 0 


0.0 


0.0 


0.0 


0.0 


0.0 


0.0 


0.0 


0.0 


0.0 


# 


timestep: 0. 


10 seconds 






















0.0 


0.0 


0.0 


0 . 0 


45.0 


0.0 


0.0 


0 . 0 


0 . 0 


0.0 


0.0 


0.0 


0 . 0 




60 . 0 


0 . 0 


0 . 0 


0.0 


45.0 


0 . 0 


0.0 


0 . 0 


0.0 


0.0 


0.0 


0.0 


0.0 




180 . 1 


0 . 0 


0 .0 


0 .0 


45.0 


0.0 


0.0 


0.0 


0.0 


0.0 


0 . 0 


0 . 0 


0.0 
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D. make auv uvw tar Archive Creation Script 



# make_auv_uvw_tar : NPS AUV Underwater Virtual World archive creation script 

# Don Brutzman 20 OCT 94 

echo create auv underwater virtual world distribution tar file auv-uvw . tar . Z 

# Notes: 

# do not make this tar on gravyl , it doesn't have enough memory for 'speech' 

# ensure mission . script . siggraph was the latest (best) mission run 

# this script is not included in the distribution. 

# NPS Autonomous Underwater Vehicle (AUV) World-Wide Web Home Page 

# f tp : //taurus .cs.nps. navy.mil/pub/mosaic/auv. html 

cd /n/dude/work/brutzman 

echo 'removing old files...' 
rm - f auv-uvw. tar 

rm -f auv-uvw . tar . Z 



# Standard e-mail report becomes the README file 



rm -f 

cp execution/mission . output . email 

chmod - w 

tar -cvf auv-uvw. tar 

rep auv-uvw .virtual -world . README 

chmod +w 
rm - f 



auv-uvw . virtual -world . README 
auv-uvw . virtual -world . README 
auv-uvw . virtual -world . README 
auv-uvw . virtual -world . README 
taurus : -ftp /pub/ auv 
auv-uvw . virtual -world . README 
auv-uvw . virtual -world . README 



rm -f 

cp execution/auv-uvw . INSTALL 

chmod -w 

rep auv-uvw .virtual -world . INSTALL 

tar -rvf auv-uvw. tar 
chmod +w 
rm -f 



auv-uvw. virtual -world. INSTALL 
auv-uvw . virtual -world . INSTALL 
auv-uvw .virtual -world. INSTALL 
taurus : -f tp/pub/auv 
auv-uvw . virtual -world . INSTALL 
auv-uvw .virtual-world . INSTALL 
auv-uvw .virtual -world . INSTALL 



rm -f 

cp execution/auv-uvw . REMOVE 

chmod +x 

tar -rvf auv-uvw. tar 
rm 



auv-uvw .virtual -world. REMOVE 
auv-uvw .virtual -world. REMOVE 
auv-uvw . virtual -world . REMOVE 
auv-uvw. virtua 1- world. REMOVE 
auv-uvw .virtual -world. REMOVE 



rep 



execution/mission . script . HELP taurus : ~ ftp/pub/ auv 



tar 



-rvf auv-uvw. tar auv-uvw .viewer 



tar -rvf 
tar -rvf 
tar -rvf 
tar -rvf 
tar -rvf 
tar -rvf 
tar -rvf 
tar -rvf 
tar -rvf 



auv-uvw .tar 
auv-uvw .tar 
auv-uvw .tar 
auv-uvw . tar 
auv-uvw . tar 
auv-uvw . tar 
auv-uvw .tar 
auv-uvw . tar 
auv-uvw . tar 



auv-uvw/viewer 
auv-uvw/viewer . C 
auv - uvw/Make f i le 
auv-uvw/ overhang . rgb 
auv-uvw /auv . iv 
auv-uvw/Jason . iv 
auv-uvw/Platf orm . iv 
auv-uvw/Testtank . iv 
auv-uvw/testtank . C 



tar 

tar 

tar 



-rvf auv-uvw. tar auv.html 
-rvf auv-uvw. tar mbone.html 
-rvf auv-uvw. tar nps_mosaic.html 



tar -rvf auv-uvw. tar . sd . t cl . auv-uvw 
tar -rvf auv-uvw. tar .mailcap . auv-uvw 
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tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


chmod -w 




tar 


-rvf 


auv-uvw . tar 


chmod 644 


tar 


-rvf 


auv-uvw .tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 


tar 


-rvf 


auv-uvw . tar 



. cshrc-excerpt .auv-uvw 

execution/execution 
execution/execution . c 
execution/parse_f unctions .c 
execution/globals . c 
execution/globals . h 
cxecution/dcf ines .h 
execution/ermo . h 
execution/modes . h 
execution/setsys .h 
execution/sgstat .h 
execution/Makef ile* 

execution/mission .script . HELP 
execution/mission. * 
cxccution/mission . script . HELP 

execut ion/auv j)lot .gnu 
execution/auv_plot_l_second .gnu 
exe cut ion/print^ lots 
execution/disbndge . c 
execution/os9sender . c 
execution/os9server . c 

dynamics /dynamics 
dynamics /Make file 
dynamics/* . H 
dynamics/* . C 



echo 'remove speech/numbers . au originating from high replication counts...' 



rm 


-f 


dynamics/speech/2? .au 


rm 


-f 


dynamics/speech/3? .au 


rm 


-f 


dynamics/speech/4? .au 


rm 


-f 


dynami cs/ speech/ 5 ? . au 


rm 


-f 


dynamics/ speech/ 6 ? . au 


rm 


-f 


dynamics/speech/7? .au 


rm 


-f 


dynamics /speech/ 8? . au 


rm 


-f 


dynami cs/ speech/ 9 ? . au 


rm 


-f 


dynamics/speech/l ? ? .au 


rm 


-f 


dynamics/speech/2?? . au 


rm 


-f 


dynami cs/spee ch/3 ? ? . au 


rm 


-f 


dynamics/ speech/ 4 ? ? . au 



echo 'the speech directory can get very large and occasionally needs pruning., 
tar -rvf auv-uvw. tar dynamics/speech/* . au 

tar -rvf auv-uvw. tar DIS.mcast/* 

# The following files are provided separately to reduce tar file size. 

# See the auv home page for remote access instructions 

# ftp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

# tar -rvf auv-uvw. tar execution/AUV_telemetry .ps 

# tar -rvf auv-uvw. tar aaai92ws .ps . Z 

# tar -rvf auv-uvw. tar dynamics/www 

# tar -rvf auv-uvw. tar execution/gnuplot 



Is -1 auv-uvw. tar 

echo 'compressing tar file...' 

compress auv-uvw. tar 

Is -1 auv-uvw. tar .Z 

echo 'rep auv-uvw . tar . Z taurus : -ftp/pub/auv ' 
rep auv-uvw . tar . Z taurus : -ftp/pub/auv 

echo "archive information on taurus anonymous ftp server:" 
rsh taurus 'Is -1 ~f tp/pub/auv/ auv-uvw . tar . Z' 
echo ' make_auv_uvw_tar complete.' 
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E. auv-uvw. virtual-world. REMOVE Archive Removal Script 



# auv-uvw .virtual -world . REMOVE archive removal shell script 

# To run this script, please type the following at the unix prompt: 

# chmod +x auv-uvw .virt ual- world .REMOVE 

# source auv-uvw .virtual -world .REMOVE 

# Don Brutzman 19 OCT 94 



# NPS Autonomous Underwater Vehicle (AUV) World-Wide Web Home Page 

# f tp : //taurus . cs .nps .navy.mil/pub/mosaic/auv.html 

echo 'remove auv underwater virtual world distribution...' 
echo 'be sure you run this from the directory you installed it' 
echo ' (ordinarily your home directory) ' 



rm -f 
rm -f 



auv-uvw . tar 
auv-uvw . tar . 2 



echo 'warning: unaliasing rm...' 

unalias rm 

rm -f -r execution/* 

rm -f -r dynamics/* 

rm -f -r auv-uvw/* 

rm -f -r DIS.mcast/* 

# remove any inadvertantly created .dot files 
rm -f -r execution/.* 

rm -f -r dynamics/.* 

rm -f -r auv-uvw/.* 

rm -f -r DIS.mcast/.* 



rmdir 

rmdir 

rmdir 

rmdir 



execution 
dynamics 
auv-uvw 
DIS .mcast 



rm 

rm 

rm 

rm 

rm 

rm 

rm 



-f 

-f 

-f 

-f 

-f 

-f 

-f 



rm -f 
rm -f 
rm -f 
rm -f 



auv-uvw .viewer 
auv . html 
mbone . html 
nps_mosaic .html 
. sd . tel . auv-uvw 
.mail cap . auv-uvw 
. cshrc-excerpt .auv-uvw 

auv-uvw .virtual -world .README 
auv-uvw. virtual -world. INSTALL 
auv-uvw .virt ual -world .REMOVE 
auv-uvw . * 



echo 'restoring .sd.tcl with your saved .sd.tcl.old, please confirm. 

echo cp .sd.tcl.old .sd.tcl 
cp .sd.tcl.old .sd.tcl 



echo ' auv-uvw .virtual -world . REMOVE complete. 
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HI. NPS AUTONOMOUS UNDERWATER VEHICLE EXECUTION LEVEL 



A. Introduction 

The execution level of robot software is the hard real-time process which 
controls propellers, thrusters, plane surfaces, sonars and other sensors. Hardware 
interfaces to the robot microprocessor are controlled by the execution level. The 
execution level is further tasked with interprocess communications with the tactical 
and strategic levels of the robot architecture (Byrnes 93), as well as the virtual world 
when operating in the laboratory. 

The execution code presented here has been tested under a wide variety of 
laboratory conditions. Regrettably much of the interface code to actual vehicle 
hardware has not been tested since early 1994 when a battery explosion caused major 
damage to the NPS AUV II. Hardware repairs from February through October 1994 
were required before in-water testing resumed. Nevertheless pre-explosion hardware 
interface source code has been retained in place and clearly identified through 
comments and compile flags. Future work includes verifying hardware interface code 
(Marco 95), updating the execution code and then running the combined virtual 
world/real world version of the execution level in the water. 

An important benefit of a robot connected to an integrated simulator 
(Brutzman 92a, 92b, 92c) or underwater virtual world (Brutzman 94) is that robot 
software development can continue independently of actual robot readiness. 
Approximately half of the execution level source code presented here was developed 
during the eight month period that the NPS AUV II was out of commission. This 
advantage alone proved to be strong justification for the value of providing a network 
connection to the actual robot microprocessor and software. The essential 
requirement for integrated simulation in an underwater robotics research and 
development program has been subsequently confirmed by other researchers 
(Trimble 94) (Kuroda 94). 
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Another important strength of the robot software is that the Kernigan and Richie 
variant C language source code is able to compile correctly under two dissimilar 
operating systems: SGI Irix 5.2, a Unix variant, and OS-9 version 2.3, a real-time 
operating system that includes features similar to both Unix and DOS. Compiler 
directives, customized makefiles and command line aliases are used to provide this 
cross-platform portability. Network communication via sockets has also been made 
compatible between these two operating systems. This flexibility has been invaluable 
for rapid and effective software development. One example out of many is that 
diagnosis and debugging tools under Unix are far more robust than under OS-9, 
permitting detection and correction of subtle difficulties not detectable by the 
OS-9 C language compiler and linker. 

Future work includes upgrading execution and tactical level communication 
links. Currently communications between levels use serial and parallel ports, a highly 
customized and error-prone hardware arrangement. By adding Ethernet connectivity 
to the tactical level computer, all software levels will be able to communicate via 
sockets regardless of what networked processor or workstation they might physically 
reside on. Additional long term goals include extending Internet protocol 
compatibility to acoustic telemetry communications. Ultimately underwater robots 
will be independent nodes on the Internet regardless of whether they are in the 
laboratory, operating with a tether or swimming freely and autonomously within an 
acoustic local area network (ALAN). The sophistication of virtual worlds will grow 
closer to that of the real world as robots and researchers operate in each. 
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// 



B. mission, script. HELP Robot Execution Level Mission Script Syntax 

// 



-/execution/mission . script . HELP 11 October 94 

Mission script syntax for NPS AUV execution level control in 

the NPS AUV Underwater Virtual World 
Don Brutzman brut zmanfcnps . navy . mil 



// - // 

This file describes how to change and create NPS AUV mission script files. 

Files and the 'execution' program are in the -/execution subdirectory. 

To run a new mission, copy an existing mission file over file 
' mission . script ' or edit the mission . script file for a new mission. 

Example: unix> cd execution 

unix> cp mission . script . siggraph mission . script 
unix> execution remotehost fletch.cs.nps.navy.mil 



Script commands are read by AUV execution level (execution . c) 

from the "mission . script " default file at the start of each mission. 

Some of the following commands will also work when invoked from the command 
line upon execution. 



Here are script keywords (and synonyms) that are currently recognized: 



i 



Keywords Parameters 
Synonyms (optional] 
+ 



i 



Description 

(units in feet or degrees as appropriate) 



HELP 

•> 

/? 



Provide a list of available keywords 
(as specified in this HELP file) . 



REMOTEHOST hostname tells execution level to open socket to virtual world 

REMOTE hostname which is already executed and waiting on 'hostname' 

REMOTEHOST is a command line switch. Example: 
unix> execution remotehost fletch.cs.nps.navy.mil 



// 

/* 

# 



comments follow on this line which are not executed 
note comments will still be spoken if AUDIO-ON 
pound sign also indicates a comment if in first column 



WAIT 

execute) 

RUN 



# Wait (or run) for # seconds (letting the robot 

# prior to reading from the script file again 



TIME # 
WAITUNTIL # 
PAUSEUNTIL # 



Wait (or run) until robot clock time # 

(letting the robot execute its current orders) 
prior to reading from the script file again 



TIMESTEP # 

TIME-STEP # 



change default execution level time step interval 
from default of 0.1 sec to # sec 



PAUSE 



temporarily stop execution until <enter> is pressed 
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K EA^T IME 
REAL-TIME 


run execution level code in real-time 

(busy wait at the end of each timestep if time remains) 


NOREALTIME 
NO- REALTIME 
NuNREALTIME 
NOWAIT 
NO- WAIT 
NOPAUSE 
NO- PAUSE 


run execution level code as quickly as possible 


MISSION filename 
SCRIPT filename 
FILE filename 


Replace 'mi ssion . script ' with 'filename' and start 
the new mission 


KEYS jAE I 
KEY SOAR:- ON 


read script commands from keyboard 


KEYBOARD- OFF 
NO- KEYBOARD 


read script commands from mission . script file 


XI 7 

,r 

EXIT 
REPEAT 
RESTART 
COMPLETE 
<ec. f> marker 


do not execute any more commands in this script, but 
repeat the mission again if LOOP-FOREVER is set 


K ILL 

SHUTDOWN 


same as QUIT but also shuts down socket to virtual world 
'dynamics' process. 


R ?K *» i # * ] 

S PEEL ft ( ft ft ] 

PROPS * [ft*] 

PROPELLERS' * [ft*j 


Set ordered rprr. values to ft for both propellers 
[ or independently set left & right rpm values 
to ft and ftft respectively] 

maximum propellor speed is +- 70C rpm => 2 ft/sec 


COURSE ft 

HEADING 

YAW « 


Set new ordered course (commanded yaw angle) 
ft 


TURN ft 

CHANGE- COURSE * 


Change ordered course by ft degrees 

(positive ft to starboard, negative ft to port) 


RUDDER ft 


Force rudder to remain at ft degrees 


DEADSTICKRUDDER [ft] 


Force rudder to remain at 0 [or ft] degrees 


DEPTH ft 


Set new ordered depth (commanded z) 


PLANES ft 


Force planes to remain at ft degrees 


DEADSTICK PLANES [ft] 


Force planes to remain at 0 [or ft] degrees 


THRUSTERS-ON 

THRUSTERS 

THRUSTERON 

THRUSTERSON 


Enable vertical and lateral thruster control 


NOTHRUSTER 
NOTHRUSTERS 
THRUSTERS -OFF 
THRUSTERSOFF 


Disable vertical and lateral thruster control 


ROTATE ft 


open loop lateral thruster rotation control 
at ft degrees/sec 
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NOR 07ATE 
RCTA7E0FF 
R 0 * A , l.- OF 


disable open loop lateral thruster rotation control 


LATERAL 


# open loop lateral thruster translation control 

at # ft/ sec 

(positive is to starboard, maximum is 0.5 ft/sec) 


NOLATERAL 
LA7ERAL0FF 
^ A . r. r. A.L -OFF 


disable open loop lateral thruster translation control 


POSITION # ## ### 

LOCATION # ## 


reset vehicle dead reckon position to 
(x, y, z) = (#, ##, ###) 


ORIENTATION # ## ### 
ROTATION # ## ### 


reset vehicle orientation to 
(phi, theta, psi) = (#, ##, ###) 


CONTINUE 

GO 


continue reading script & executing, no action performed 


STE v 

SINGLE-STEP 


loop for another timestep prior to reading script again. 
Particularly useful in keyboard mode. 


TRACE 
TRACE -ON 


enable verbose print statements in execution level 


T RAC EOF F 

NO TRACE 
NO- TRACE 


disable verbose print statements in execution level 


L00PF0RE7ER 
LOOP- FOR EVER 


repeat current mission when done. 

each repetition is called a 'replication.' 


LOC PONCE 


do not LOOP FOR EVER , stop when end of script is reached 


L . I PFILEEACKUP 
LOOP- FILE-BACKUP 


back up output files during each loop replication 
to permit inspection while new files are written 
the backup files are in execution directory: 
output . telemetry . previous & output . l_second . previous 


ENTERCONTR 0LC0NSTAN7S 


start a keyboard dialog to enter 



ENTER -CONTROL -CONSTANTS revised control algorithm coefficients 



SLI FT NGMODE COURSE 
SLI LING-MODE- COURSE 


Sliding mode course control algorithm (not yet working) 


SLIDINGMODEOFF 

SLIDING-MODE-OFF 


Disable sliding mode course control algorithm 


SONARTRACE 


enable verbose print statements in execution sonar code 


SONARTRACEOFF 


disable verbose print statements in execution sonar code 


SONAR INSTALLED 


sonar interface hardware cards are installed, use them 


PARALLELPORTTRACE 


enable trace statements for parallel port communications 


AUDIBLE 
AUDIO 
AUDIO- ON 
SOUND -ON 
SOUNDON 
SOUND 


enable text- to-speech audio output 
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c T L. EN~ 


disable text- to-speech audio output 


N 2S OUNL 

SCUNDOFF 

SOUND-OFF 

AUDI OOF F 

AUDIO-OFF 

QUIET 




EMAIL 
EMAIL -ON 
E - KA I L 
E-MAIL-ON 
EMAILON 


ask user for electronic mail address at mission start, 
send user an electronic mail report at mission finish 


EMA I - .'F : 
EMAIL- OFF 
E-MAILCFF 
E-MAIL-OFF 
NC- E-MAIL 
NO- EMAIL 
NC- E-MAIL 
NC EMAIL 


disable electronic mail address query feature 


WAYPOINT *X #Y I #2] 

W.A Y. : 01 NT - OK *>. * Y [ * 2 ] 


Point towards waypoint with coordinates (#X, #Y) 
(depth *2 optional). Leave waypoint control by 
ordering course, rudder, si iding-mode, rotate or 
lateral thruster control. 


WAY POINT FOLLOW 


Set mode to arrive at each waypoint before reading the 
next mission script command, i.e. continue towards each 
waypoint for however long it takes to reach the standoff 
distance before pausing to read the next command. 


WAY PC I NTFCLLOWOFF 
WAYPOINT- FOLLOW-OFF 


Disables WAYPOINTFOLLOW mode 


STANDOFF * 
STAND-OFF # 
STANDGF FD I STANCE # 
STANDOFF- 1 1 STANCE * 


Change standoff distance for WAYPOINT- FOLLOW and HOVER 
control 


H VO Eh ( * X * Y [ *» 2 ] ] 


! Hover using thrusters and propellers for longitudinal 
and lateral positioning at specified or previous 
waypoint 


H .YEP #X *Y #2 


[ ^orientation j [ fcstandof f- distance j 

Uses WAYPOINT control until within #standof f -di stance 
of HOVER point {#X, #Y, #2) , then switches to 
HOVER control with (optional) final #orientation 

Full speed {700 RPM) port & starboard is used if 
AUV distance to WAYPOINT is > #standof f-distance + 10' , 
then slows to 200 RPM until within #standof f-distance, 
then HOVER control. 
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c. 



execution. c Robot Execution Level Real-Time Control Program 



Pi ogram 
Authors 



Revi sed 



System : 
Compiler : 

Compilation : 

[ 68020 ] 

[ 68030 ] 

[Inx ] 

ExeC’j tier.: 

[ I r 2 x ] 



PI ot ting : 






Inscription : 



» » »«****i»»«**«»********* 



*****»****************»********/ 



execution. c AUV execution level program 
Don Brutzman, Dave Marco & Walt Landaker 
28 October 94 

AUV Gespac 68020/68030, OS-9 version 2.4 
Gespac cc Kernighan & Richie (K&R) C 

ftp> put execution. c 
auvsiml> chd execution 
auvsiml> make -k2f execution 
auvsiml> make execution 

fietch> make execution 



fletch> cd execution 

fletch> execution remote dynamics-hostname 

where dynamics-hostname is the IP name of the host running 
the dynamics (virtual world} program 

see gnuplot scripts ' auv_j?l ot . gnu ' and ' auv__plot_l_second . gnu ' 
fletcn> gnuplot auv_plot.gnu 

gravy 1: -brutzman/ execution>> lint -lm execution. c 

lint -lm -Iglobals.h -Idefines.h globals.c parse_functions . c \ 
execution . c 

fletch> make warnings 

closed loop for operation during vehicle in-water 
missions as well as in virtual world 



Active changes: Don Brutzman working lab/virtual world networked version 

& tactical level interface 

Dave Marco working vehicle code 

& interfacing physical devices 

Future work: Update digital <==> analog access for new vehicle hardware 

Retest code after vehicle repaired 

Sonar/altimeter integration code reintegrated/retested 
Audio strings seem to be generated differently by OS-9 
standardize parsing of command line and script commands 
finish sliding mode control 

change serial /parallel comms to sockets once 
tactical level gets an Ethernet card 



Testing interprocessor connections: 
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parallel port 



serial port 



Interfaces : 

Telemetry sent 
Telemetry received 



/P OS-9 auvsiml> mfi_a3 

LPT1 : DOS auvsim2> portfix 

> print filename.txt 

{ /T1 ) /TT OS-9 auvsiml> wr2tl then write text 

OS-9 auvsiml> rdtla then read text 

DOS auvsim2> C:\COMM\PROCOMM 

then <alto> for chat mode 
<al tF10> help , <al tX> exit 



via serial port /tt [== /tl at high baud rate] 
via parallel port /P 



reler ietry is optionally passed to /'from tactical level running on 80386 



Reads files : 
Writes files: 



miss: on . ini t [mission initialization data file ] 
output. data [vehicle telemetry state vector data] 
output. auv [tactical order/executive report log] 



Sonar commands / rep! ies via device pert /t3 

Note that %F double formats are used instead of %lf on scanf() and sscanfO 
calls for OS-9 compatibility 



********«** v ****«** t ** i 



# include "g locals . h “ 
^include “defines. h" 



/”*** ****************************** **********/ 

/* function prototypes */ 

/* is there so me way to put parameter specifications in the prototypes?? */ 



only if we buy the ANSI C compiler from Microware (or shift to VxWorks) */ 



void 


closed_loop_cont rol_modul e 


0 


void 


get_cont rol_const ants 


0 




Gtrp t h 


0 


QGuLic 


calcula te_psi 


0 


void 


zerc_gy rc_data 


0 


vc.d 


ini tial ize_dacs 


0 


vc. d 


initial i ze_adcs 


0 


vc.c 


contrcl_surface 


0 


VC . sj 


rudder 


0 


void 


planes 


0 


voio 


mar n_mo t o r s_o f f 


0 


void 


alive 


0 


void 


record_da ta_on 


0 


void 


record_data_of f 


0 


void 


record_da ta 


0 


douole 


rol l_angle 


0 


double 


pi tch_angle 


0 


double 


heading 


0 


double 


roll_rate_gyro 


0 


double 


pit ch_r a t e_gy r o 


0 


double 


yaw_rate_gyro 


0 


double 


s t bd_mo t o r_rpm 


0 


double 


port_motor_rpm 


{) 


double 


get_speed 


0 


void 


get_ini t_avg 


0 


void 


get_avg_rng 


0 


void 


open_devi ce_pa ths 


0 ; 
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void 




close_device_pa ths 


0 


void 




read_para llel_port 


0 


void 




da c 1 


0 


void 




dac2b 


0 


int 




add 


0 


ir. r 




adc2 


0 


V O 1 G 




I ni t_PortA 


0 


void 




Ini t_PortB 


0 


unsigned 


char 


Read_PortA 


0 


unsigned 


char 


Read_PortB 


0 


unsigned 


short 


Read_PortAB 


0 


void 




set_bsyA 


0 


void 




rst_bsyA 


0 


int 




ck_sta 


0 


void 




center_sonar 


0 


char 




query_sonar_l_reply 


0 


void 




set_s tep_si ze 


0 


void 




t ty_mode 


0 


void 




print_valid_key words 


0 


V _ 11 




open_vi rtual_world_socket 


0 


void 




shutdown_virtuai_world_socket 


() 


'.■ * -d 




sencLda ta_on_vi rtual_world_socket 


0 


V O 1 G 




get_stream_f rom_virtual_world_socket 


0 


double 




degrees 


0 


dcutle 




radians 


0 


double 




normal i ze 


( ) 


d-7-ble 




normal i ze2 


0 


d c u l - e 




radian_normalize 


0 


U O U U - tr 




radian_normal i ze2 


0 


void 




clamp 


0 


dC'UDa r 




a t a r. -l. 


0 


dou 1:1- 




sinh 


0 


double 




cosh 


0 


dour 1 e 




tan 1 'j 


0 


dOubl T 




sign 


0 


from pars 


:e_f unct ions . c 




ern void 




parse_command_line_f lags 


0 


ern void 




parse_jnission_script_commands 


0 


ern void 




parse_missi on_stnng_commands 


0 



V 



^*»»****»**ir»»**»»*»'**it'»»»'****»*-*»'****itii'*'**********it****tr**tr********»**»*******y 

/■* 0£- 9 ~ specific function compatibility */ 

* ; f defined ( sgi ) 



void 

void 

double 

int 

int 



tsieep (unsigned svalue) ( /* null body »/) 

_sysdate (format, time, date, day, tick) 

int format, *time, *date, *tick; short *day; 

{ /* null body */} 

pow (xx, yy) J* power function */ 

double xx, yy; 

{return exp (yy * log (xx) ) ; } 

_gs_rdy (path) int path; { return 0; } /* bytes waiting on path */ 
_gs_opt (path, buffer) 
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int path; struct sgbuf ‘buffer; 
{ return 0; } 



ss_opt (path, buffer) 

int path; struct sgbuf ‘buffer; 

< return 0; } 

/***»****★*★»*»*****★**★****★*■**•*•******•*■**★*★**★★****************************** j 
/»»»»»»»»*•*•»'*•»»**»»**«•*»*«■**★★*»***★»******»***•***************************★*★★* j 



*endi : 



ip. c argc; char ‘*argv; /* command line arguments */ 

( 

if (TRACE LL DISPLAYSCREEN) printf ('[start main: execution ] \n" ) ; 

mrcpy ( v: imual_wcrld_remote_host_name, VIRTUAL_WORLD_REMOTE_HOST_NAME) ; 
dt = TIMECTEP; 

parse_command_line_f lags (argc, argv); 

ge:_ccnt rc Inconstant s (); 

mi t; ali ze_dacs (>; 
inrt;alize_adcs (); 

open_de v i c e_pa t n s (}; 

recc rd_dat a_or. (;,* /’ Open files for data logging */ 

open .virtual _w c r i d_soc k e t (j; /* open connection to virtual world */ 

if :strlen (buffer) > 0) 

se:-jb_data_on_.vi rtua l_world_socket (); /* SILENT? send to sound driver */ 

strcpy (buffer, " A U V virtual world socket is open"); 

send_dat a_on_v; rtual_world_socket (); /* buffer containing message sent */ 

dc /* wn.le ( LOCATIONLAE LL LOOPFOREVER , */ 

t /* indefinite repeat loop for long-duration lab testing */ 

* */ 

if . LI SPLAY SCREEN ) 

{ 

if (LOCATIONLAE LL (EMAIL == TRUE)) 

( 

strcpy (buffer, " Please Enter Your E-mail Address'); 

send_data_on_virtual_worldnSocket /* buffer containing msg sent */ 

printf ("%s *“ HERE “*: ", buffer); 

strcpy (email_address , "'); 
gets (emsil_address) ; 

sprintf (buffer, "Thanks %s\n', emai l_address ) ; 
printf ("%s\n", buffer); 

send_data_on_virtual_world_socket (); /* buffer containing msg sent */ 

if ((int) (strlen (emai l_address) > 2) && 

(strcmp (emai l_address , 'brutzman') != 0) && 

(strcmp (email_address, 'BRUTZMAN') != 0)) 

( 

emailaddressfile = fopen ( EMAI LADDRESS FILENAME, 'a'); /* append */ 

fpnntf (emailaddressfile, '%s\n', email_address ) ; 
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fclose (emailaddressf ile) ; 
i 

/ 

if LOCATIONLAB = = FALSE) 

alive i 1 G , start_awel 1 ) ; f* Wag fin every 10 seconds for total duration 

of start_dwell seconds */ 

printf(* Position A T JV for Directional Gyro Offset Measurements ■ ) ; 
printf(* Rate Gyro zero measurement \n m ) ; 

printf( w Hit <Enter> on AUV When Ready *** Here ! ***\n B ); 
answer = getchar (); /* pause */ 

) 

printf( # OK!! Starting the mission . \n ■) ; 



parse_mission_script_commands (); /* read initial script orders */ 

zerc_gyrc_data (); /* Get daily zeros for gyros */ 

if (SONARI INSTALLED) center_sonar (); /* must have open_device_paths 1st */ 

sticpy (buffer, " ,,,,“); /* pause */ 

send_data_on_virtual_world_socket (); /* buffer containing msg sent */ 

strep y 'Duffer, " A U V is starting"); 

send_data_on_virtual_world_socket (); /* buffer containing msg sent */ 

• * Initialization of closed loop parameters */ 

be f fer_inde>: = 0; 

z«rlemetry_records_saved = 0; 

rr.ission_Ieg_counter = 0; 

ena_test = FALSE; 

wrap_count = 0; 

t = 0.0; 



-*/ 

/’ Main program operational loop code */ 

if (TRACE &£. D1SPLAYSCREEN) 

print f ("[Starting main program operational loop code... ]■); 
previousloopclock = clock (); 



/* */ 

wnile (ena_test == FALSE) /* this is the realtime main operational loop */ 

/* when end_test == TRUE then loop is done */ 

{ 

cl osed_l oop_control_module (); /* closed loop code is here «<«««<*/ 



) /* end of real-time main operational loop */ 

/• */ 



/* lab version may repeat forever for long-duration testing*. */ 

replication_count ++; 

if (LOCATIONLAB && LOOPFOREVER && DISPLAYSCREEN) 

( 

printf C\n[LOOP FOREVER enabled, next loop is replication %d...]\n - , 
repl ication_count ) ; 

sprintf (buffer, ■ LOOP FOREVER enabled, next loop is replication - ); 
send_data_on_virtual_world_socket (); /* buffer msg sent */ 

sprintf (buffer, ■ %d - , replication_count ) ; 

send_data_on_vi rtual_world_socket (); /* buffer msg sent */ 

) 

if (LOCATIONLAB && LOOPFOREVER) 

{ 
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/* reset amount of time to wait for next command */ 
t ime_next_command = 0.0; 
t = 0.0; 

if (DISPLAYSCREEN ) 

{ 

print f ( " \nLoopf orever reset time: [ time_next_command = 0.0] "); 

pnntf ( ■ [t = 0.0) \n " ) ; 

) 



if (LOOPF ILEBACKUP) 

record_data_of f (); 



*-f def 



fcej.se 



rr. J- I 



ned (sgi ) 






pi i n: f 


■ " m. 




sy s t eiTi 


{ “ rm 




pri nt f 


Cep 


mission . output . telemetry 


system 


Cep 


mission .output . telemetry 


printf 


( " rm 




system 


( " rm 




printf 


( "cp 


mission . output . l_second 


system- 


l "cp 


mission . output . l_second 


print f 


< "del 




system 


i " del 




print f 


i "copy 


mission . output . telemetry 


system 


“copy 


mission . output . telemetry 


print f 


i “del 


system 


: "del 




r v- - r- *■ f 
t-' - - J * - *■ 


' "copy 


mission . output . l_second 


S j Sttri: 


"copy 


mission . output . l_second 



output . telemetry .previous\n" ) 
output . telemetry .previous" ) 
output . telemetry . previous \n" ) 
output . telemetry .previous" ) 
output . l_second . previous\n" ) 
output . l_second . previous " ) 
output . l_second .previous\n" ) 
output . l_second . previous ’ ) 

output . telemetry' .previous\n" ) 
output . telemet ry .previous" ) 
output . telemetry .previous\n" ) 
output . telemetry .previous" ) 
output . l_second . previous\n " ) 
output . l_second . previous " ) 
output . l_second . previous\n " ) 
output . l_second . previous " ) 



l ^ JLr. . I , 



strcpy (buffer, " telemetry data backup complete"); 
sena_data_on_vi rtual_world_socket (); /* buffer msg sent */ 



<rlse / * don't bother backing up* most recent results 



II i LOCA . I ON LAB & t* LOOP FOREVER ) 



rewind (a uvtextf ile j ; 
rewind ( auvda ta f i le ) ; 
if TRACE && DISPLAYSCREEN) 

print! ( " (auvtextf ile & auvdatafile rewound to "); 
pnntf i " output . data . previous & output . auv . previous ] \n" ) ; 
strcpy (buffer, " telemetry data backup skipped"); 
sen d_d a t a_o n_v i r t u a 1 _wor 1 d_s o c k e t (); /* buffer msg sent */ 



) 



fflush (auvscript f ile) ; 
if (fclose {auvscript file) = = 0) 

( 

if (DISPLAYSCREEN) 

printf ("(success closing auvscriptf ile mission . script .backup] \n* ) 

) 

else if (DISPLAYSCREEN) 

printf (’[failure closing auvscriptf ile mission . script .backup] \n" ) 

fflush (auvordersf ile) ; 
fclose (auvordersf ile ) ; 



/* 

#if defined(sgi) 
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if (TRUE && DISPLAYSCREEN) 
print f M rm 
system ( " rm 

printf Umv 
sy stem ( " mv 



mission . output 
mission . output 



#ei se 



printf (“del 



♦ endi f 

*/ 



system 

printf 

system 

printf 

system 



("del 

(“copy mission .output 
(“copy mission . output 
(“del mission . output 
(“del missi on . output 



mission . output .orders\n“ ) ; 
mi ssion . output . orders “ ); 

. orders . backup mission . output . order s\n“ ) ; 
, orders . backup mission . output . orders “ ); 

mission . output . order s\n“ ) ; 
mission. output .orders* ) ; 
. orders . backup mi ssion . output .orders\n“ ) ; 
. orders .backup mi ssion . output . orders “ ); 

. orders .backup\n\n" ) ; 

. orders . backup" ); 



-------- orders 

♦ if defined(sgi) 

sprint f (buffer, “rm%s\n“, AUVORDERSFILENAME) ; 

♦ el se 

sr i ::i: f (buffer, “del %s\n", AUVORDERSFILENAME); 

♦ er.d: f 



: f (DISPLAYSCREEN: printf (“ ? oS\n H , buffer); 
sy stem ( bu f f e r / ; 

♦ if defined(sgi) 

sprint f (buffer, “mv %s. backup %s\n\ AUVORDERS FILENAME, AUVORDERSFILENAME) ; 

♦ el se 

sprint f (buffer, “copy %s. backup %s\n“, AUVORDERSFILENAME, AUVORDERSFILENAME); 

♦ end: f 

: f 'DIE PL AY SCREEN ' printf ("%s\n“, buffer) > 
system (buffer, ; 

# i : defined ;'sg: 

♦ e 1 se 

sprint f (buffer, “del %s \n“, AUVORDERSFILENAME); 

if 'DISPLAYSCREEN . printf (“%s\n", buffer); 
system (buffer); 

♦ endi f 



/* 



e-mail 



*/ 



♦if defined (sgi ) 

sprint f (buffer, “rm%s\n“, AUVEMAILFILENAME) ; 

.ri r i r: t f (buffer, “del %s\n\ AUVEMAILFILENAME); 

♦ enc: t 

if 'DISPLAYSCREEN ) printf ("%s\n", buffer); 
system (buffer); 



♦ :f defined(sgi) 




sprint f (buffer, “cp %s 


%s\n* 


♦ else 




sprlntf (buffer, “copy %s 


%s\n“ 


♦ endi f 




if (DISPLAYSCREEN) printf 
system (buffer); 


r %s\n“ 


♦if defined(sgi) 




sprintf (buffer, “cat %s 


>> %s\n“ 


♦ else 




sprintf (buffer, "list %s 


» %s\n" 


♦endi f 




if (DISPLAYSCREEN) printf 
system (buffer); 


( " %s\n" 



AUVINFOFILENAME, AUVEMAILFILENAME) ; 
AUVINFOFILENAME, AUVEMAILFILENAME); 
buffer) ; 

AUVSCRIPTFILENAME, AUVEMAILFILENAME); 
AUV SCR I PTF I LENAME , AUVEMAILFILENAME); 
buffer) ; 
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* : f defined •. sgi ) 

sprint f (buffer, "cat is >> %s\n", AUVORDERSFILENAME , AUVEMAILFILENAME); 

# tr . Se 

srrintf (buffer, "list %s » %s\n", AUVORDERSFILENAME, AUVEMAILFILENAME); 

*er ; di f 

if (DISPLAY SCREEN^ printf ("%s\n", buffer); 
sy s t en« (buffer) ; 

if ( ( i n r (strlen (emai l_address) >= 3) && (EMAIL == TRUE)) 

( 

sprintf (buffer, "mail %s < %s", email_adaress , AUVEMAILFILENAME); 

* : f defined (sgi j 

printf ("%s\n", buffer); 
system (buffer); 

* e 1 se 

/* system (buffer); /* e-mail not available directly on OS-9 */ 

send_data_on_virtual_world_socket (); /* buffer msg sent anyway */ 

*er.di f 

) 



/* pent it changing the vehicle mission during continuous lab testing */ 
if (LOCATIONLAE && LOOPFOREVER ; 
i 

get_cont rol_constant s (); 
previ ous 1 cop clock = clock i j ; 
record_da:a_on ( ) ; 

strcpy (buffer, " Load mission again"); 
send_data_on_virtual_world_socket () ; 

/* buffer containing message sent */ 



i while f LOCATIONLAE && LOOPFOREVER); /* end of lab infinite loop (if any) V 

/»'»*'»T»'»T'»*T'*-****'»'***'»****'ir'»*T*'*T********'»'»******'»*'»ir*«ir***'»**********ir***** j 



: - .:.y rru f f ); * all done, turn them; off V 

if ■* TRACE ka DI EPLAYSCREEN , 

printf . “i sending 'shutdown' message to virtual world dynamics] \n" ) ; 

strcpy (buffer, "shutdown"); J* must start with 'shutdown' to die */ 

s*r:.d_jr.: a_~ :\_vi r tuaI_world_socke: '}; /* buffer containing message sent */ 

shutdown _vi rtual_world_socket (;; /* close connection to virtual world */ 

cl ose_device_ paths ( ) ; 

record_data_of f (); 

if (TRACE && DISPLAYSCREEN) 

printf ("(finishing main: fflush (staout), fflush (stderr) ] \n" ) ; 

fflush ( stdout ) ; 
fflush ( stderr ) ; 

if (TRACE && DISPLAYSCREEN) printf ("(main exit: return (0)]\n"); 

*^f defined (sgi) 

if (DISPLAYSCREEN) printf ("gnuplot auv_plot_l_second . gnu\n" ) ; 

system ("gnuplot auv_plot_l_second .gnu " ) ; /* display plotted results */ 

#endi f 

return (0); /* main program exit V 
) /* end main program block, execution is complete */ 

/■*•*★**•**»**•*»■****★•*★****★★★★*******★★*★★****★**********★**★■*★**■******.*,*.***.**.*** j 
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***************-***************************»***********************»**/ 



void closed_ioop_control_module () 



if (TRACE && DISPLAYSCREEN) 

printf ("(start closed_loop_control_module) \n" ) ; 

/ * Speed Control ************************************************* 

spe^d = get_speed (); 

rpn. - (port_rpm_command -► stbd_rpm_command) / 2.0; 

clamp (& rpm, 700.0, -700.0, "rpm"); /* bound maximum RPM */ 

i: .TRACE 6.S. MSP^AY SCREEN) 

printf ("(clamp (& rpm, 700.0, -700.0, VrpmV) complete] \n" ) ; 
/* Main_Motor RPM Control ************** ************************** 

/’ note thruster use does not preclude propeller use 
if , LOCATIONLAB == TRUE) 



********, 



* ***** * j 
*/ 



per t_ rpm = port_rpm_command ; 
stod_rprr = stDd_rpm_command; 

else /* in water, propeller control */ 

pcrt_rpm = port_motor_rpm (); 
stbd_rpm = stbd_motor_rprr ( ) ; 



: f iNCT_YET_RE:M?LEMENTED) 

( 

mair._motor_del tal = f abs (rpm) - stbd_rpm; 
main_motor_del ta2 = fabs(rpm) - port_rpm; 

/* this is reset windup for proportional integral control of motor speed V 
/* in order to prevent accumulating the integral of speed error */ 

if (main_motor_del tal>50 . 0 ) main_motor_del tal = 50.0; 
if imain_motoi_del ta2>50 . 0 ) main_motcr_del ta2 = 50.0; 



) 



mair._motor_vol tl = mam_motor_vol 1 1 4 (rpm/f abs (rpm) * 0 . 2 *main_motor_del tal ) ; 
main_motor_vol t2 = main_motor_volt2 4 ( rpm/ f abs { rpm) *0 . 2 *main_motor_del ta2 ) ; 



:f : mam_motor_voj tl > 1023) main_motor_vol tl 
-i in.6;n_motor_voitl < 0) main_motor_vol t 1 
if :main_motor_vol t2 > 1023) main_motor_vol t2 
. f imain_mctor_vol t2 < 0/ main_motor_vol t2 



1023; 

0; 

1023 ; 
0 ; 



da cl ( ma i n_mo t o r_vo 1 1 1 , R I GHT_ MOTOR ) ; 
da cl (main_motor_volt2 , LEFT_MOTOR ) ; 



/* if using virtual world dynamics, network is source of values «««« */ 



phi = roll_angle () 
theta = pitch_angie () 
psi = calculate_psi () 

p = roll_rate_gyro () 
q = pitch_rate_gyro () 
r = yaw_rate_gyro () 
2 = depth () 



/* read roll angle */ 
/* read pitch angle */ 
/* Read heading */ 

/* read roll rate V 
/* read pitch rate V 
/* Read yaw rate */ 
/* Read depth * / 



/* note: in laboratory using virtual world, values above are superceded */ 



32 



/* Contrcl laws NOTE: all k_ constants must be (+) positive **** */ 

waypcint_distance = sqrt { (x - x_command) * (x - x_command) 

4 (y - y_command) * (y - y_command) ) ; 



/ w use WAYPOINTCONTROL (not HOVERCONTROL ) until within standof f_distance 
if (; HOVERCONTROL == TRUE) && (waypoint_di stance > standof f ..distance) ) 
( 

WAYPOINTCONTROL = TRUE; 

DEADSTICKRUDDER = FALSE; 

if (waypoint_di stance > standof f_di stance 4 10 . 0 ) 

{ 

port_rpm_command = 7 00; 
stba_rpm_command = 700; 
fprintf ( auvordersf lie, 

■%6.1f % 6 . 1 f c 5 . If %5 . If %5 . If % 6 . 1 f %6.1f % 6 . 1 f % 6 . 1 f %5.1f %5.1f %5.1f 
%5. If \n- , 

t , psi_command, x_command, y_command, z_command, 
port_rpm_command, stbd_rpm_command, 
rudder_command, planes_command, 
bow_lateral_thruster_command, 
sternal a teral_thruster_command , 
bow_ver ti ca l_thrust er_command , 
stern_vertical_thruster_command) ; 

} 

else 

\ 

port_rpm_command = 200 ; 
stbd_rprr_command = 200 ; 
fprintf ( auvordersf ile, 

-U.:: 16. If 15. if %5 . 1 f %5 . 1 f % 6 . 1 f % 6 . 1 f %6.1f % 6 . If %5.1f %5.1f %5 . 1 f 

\ i . 1 f '* , 

t , psi_command, x_command, y_command, z_command, 
po r t_rprr_c omma nd , s t bd_rpm_c omma nd , 
rudder command, planes_command, 

bow_ 1 a t e r a 1 _ t h ru s t e r _c omiria nd , 
stern_lateral_thruster_command, 
bow_v e r t 1 ca 1 _t h ru s t e r _c omma n d , 
stern_verticaI_thruster_command) ; 

} 



V 



else if ((HOVERCONTROL == TRUE) && ( WAYPOINTCONTROL == TRUE)) 

/’ restore proper HOVERCONTROL */ 

( 

WAYPOINTCONTROL = FALSE; 

DEADSTICKRUDDER = TRUE; 
rudder^ command = 0.0; 
ps:_command = psi_command_hover; 

pcrt_rpm_command = 0; 
s tbd_rpm_command = 0; 

fprintf (auvordersf il e, 

" %6 . 1 f % 6 . 1 f % 5 . 1 f % 5 . 1 f % 5 . 1 f % 6 . 1 f % 6 . 1 f %6.1f %6.1f %5.1f %5.1f %5.1f 

% 5 . 1 f \n " , 

t, psi_command, x_command, y_command, z_command, 
port_rpm_command, stbd_rpm_command, 
rudder command, pi anes_command , 

bow_lateral_thruster_command, 
sternal a teral_thruster_command, 
bow_vertical_thruster_command, 
stern_vertical_thruster_command) ; 

) 

if (WAYPOINTCONTROL == TRUE) 

( 

/* note that a reversed x,y calling sequence is necessary */ 

/* in order to get correct quadrant alignment */ 

waypoint_angle=normalize {degrees (atan2 (y_command - y, x_command - x))); 



33 



= waypoint_angle; 



) 

if 



f 

\n " 



psi_command 

if iTRACE ; 

{ 

printf ( "WAYPOINTCONTROL psi_command = %5.1f, ", psi_command) ; 

printf ("x = %5.1f, y = %5.1f\n", x, y); 

( (FOLLOWWAYPOI NTMODE == TRUE) (HOVERCONTROL == FALSE) && 

( ( waypoint_distance > standof f_di stance ) I I 
(fabs ( z - z_command) > standof f_distance) ) ) 

( 

if (TRACE; printf ( '[ FOLLOWWAYPOI NTMODE check ]") ; 

/* continue until WAYPOINT reached without further script orders */ 
t ime_next_command = t + 2.0 * dt; 

\ 

else /* WAYPOINT reached */ 

i 

if (TRACE) printf ( - [ FOLLOWWAYPOI NTMODE success, WAYPOINT reached]"); 
fprintf { auvordersf il e, 

it. If % 5 . 1 f o 5 . 1 f % 5 . 1 f %6.1f %6.1f %6 . 1 f % 6 . 1 f %5.1f %5.1f %5.1f 

t, psi_command, x_command, y_command, z_command, 
port_rpm_command , stbd_rpiT,_command, 
rudder_command , planes_ command, 

bow_lateral_thruster_command, 
stern_la teral_thruster_command, 
bow_ver tica l_thruster_command , 
stern__vertical_thruster_command) ; 



else if (HOVERCONTROL == TRUE) 

I 

waypcint_angle=normal ize (degrees (atan2 (y_command - y, x_command - x) ) ) ; 
track_angle = normalize ( waypoint_angle - psi ) ; 

aicng_t racr._distance = cos ( radians ( track_angle) ) * waypoint_distance; 
cross_track_ distance = - sin (radians (track_angle) ) * waypoint_di stance; 

pcrt_rpm = k_ propel ler_hover * along_track_di stance - 
k_surge_hover * u; 

s tbd_rpm = k_propel ler_hover * along_track_di stance - 
K_surge_hover * u; 



i f 



printf ( “HOVERCONTROL: \n" ) ; 

printf ( “psi_command = %5.1f, ", psi_command) ; 

printf »“x = %5.1f, y = %5.1f\n", x, y) ; 

printf ( M waypoint_distance = %5.1f, track_angle = %5.1f\n", 
waypoint_distance, track_angle) ; 

printf { "along_track_di stance = %5.1f, ", along_track_distance) ; 
printf ( “cross_track_aistance = %5.1f\n", cross_track_ distance) ; 
printf ("port_rpm / stbd_rpm = %5.1f\n", port_rpm) ; 



) 



/* Simplified PD rudders/planes control rules: --------- 

delta_rudder = k_psi * normalize2 (psi - psi_command) 

+ (k_r * r) + (k_v * v) ; 

/* tanh not provided under OS-9 C, added at end of this program 
if (SLIDINGMODECOURSE == TRUE) 



{ 



sigma = k_sigma_r * r + k_sigma_psi * normalize2 (psi - psi_command) ; 
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del ta_r udder = (3.1403 * r) + 81.9712 * eta_steering * tanh (sigma); 

} 

dept h_er ror = (z - z_command) ; 

/' constrain depth_error to + - 15.0 feet to prevent going vertical */ 

/* and enable stable pitch angle even on large depth changes */ 

clamp. (& depth_error, -15.0, 15.0, *depth_error " ) ; /* feet */ 

delta_planes - - (k_z * depth_error) 

+ (k_theta * theta) + (k_q * q) - (k_w * w) ; 

/* Dead stick means no open loop control of rudders/planes 

if (DEADSTICKRUDDER) 

{ 

del ta__rudder = rudder_command; 

} 

if iDEADST I CK PLANES) 

delta_planes - planes_command; 

) 



/ * constrain planes & rudder orders +/- 40.0 degrees */ 

clamp (& del t a_rudder , -40.0, 40. 0, ’del ta_rudder * ) ; /* degrees */ 

clamp {&.- del ca_ pi anes , -40.0, 40.0, "del ta_pianes " ) ; /* degrees */ 

/* Send r^mmands to rudders and planes •**********************************/ 

rudder ( del ta_rudder , ; 
planes (del ta_pl anes . ; 



' ’ Simplified 1 atera 1 /vert ical thruster control rules: - 

, ROTATE CONTROL == TRUE) / * open loop rotate thrusters V 

; 

1 ateral_thruster_vol ts - k_thruster_rotate * rotate_command 

* rotate_command ; 

» 

else if (LATERALCONTROL - - TRUE) /* open loop lateral thrusters */ 
i 

lateral_thruster_volts = - k_thruster_lateral * ( lateral_command) ; 

} 

else /* heading control is default */ 

i 

lateral_thruster_volts = - k_thruster_psi * normalize2 (psi -psi_command) 

- k_thruster_r * r; 

) 

verti cal_thruster_vol ts = - k_thruster_z * (z - z_command) 

- k_thruster_w * w; 

if { (THRUSTERCONTROL) II ( HOVERCONTROL == TRUE) II (ROTATE CONTROL == TRUE)) 
{ 

AUV_bow_vertical = vertical_thruster_volts; 

AUV_stern_vertical = vertical_thruster_vol ts ; 

if (LATERALCONTROL == TRUE) 

( 

AUV_bow_lateral = lateral_thruster_vol ts; /* both positive, */ 

AUV_stern_lateral = lateral_thruster_volts; /* same direction */ 

} 
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else If (HOVERCONTROL == TRUE) 

{ 

AUV_bow_l ateral = - ( - k_thruster_psi * normal 1 ze2 (psi -psi_command) 

- k_thruster_r * r) 

+ k_thruster_hover * cross_track_distance 
4 k_sway_hover * v; 

AUV_st ern_latera 1 = ( - k_thruster__psi*normalize2 (psi -psi_command) 

- k_thruster_r * r) 

-*• k_thruster_hover * cross_track_distance 
4 k_sway_hover * v; 

) 

else if { (THRUSTERCONTROL == TRUE) II (ROTATECONTROL == TRUE)) 

{ 

AUV_bow_l ateral = - lateral_thruster_vol ts; /* negative */ 

AUV_stern_l ateral = 1 ateral_thruster_volts; 

else 

( 

printf ("Thruster control logic error *** \n“); 

} 

if (TRACE && DISPLAYSCREEN) 

( 

printf ( "Thruster control ON. Pre-clamp calculated values: \n"); 
printf '" AUV_bow_vertical = %6.3f, AUV_stern_vertical = %6.3f\n", 
AUV_bow_vert ical , AUV_stern_vert ical ) ; 

printf ( " AUV_bow_lateral = %6.3f, AUV_stern_lateral = %6.3f\n", 
A T JV_bow_I ateral , AUV_stern_l ateral ) ; 



else /* thrusters disabled */ 
if (TRACE uu DISPLAYSCREEN , 

printf ( "Thruster control OFF. Pre-clamp calculated values :\n"); 
printf ( "vert ical_th rust er_vcits = %6.3f\n", 
verticai_thrnster_volts) ; 
printf i " lateral_thruster_volts = %6.3f\n", 
la teral_thruster_vol ts ; ; 



) 

AU7_bow_ve rtical = 0.0 
AU7_s tern_vert i cal = 0.0 
AUV_bow_l ateral = 0.0 
AUY_stern_l ateral = 0.0 



if (TRACE && DISPLAYSCREEN) 

{ 

printf ("Pre-sqrt thruster control calculated values:\n B ); 
printf ( "AUV_bow_vertical = %6.3f\n*, AUV_bow_vertical ) ; 
printf ( "AUV_stern_vertical = %6.3f\n", AUV_stern_vertical ) ; 
printf ( "AUV_bow_l ateral = %6.3f\n*, AUV_bow_lateral) ; 
printf ( *AUV_stern_lateral = %6.3f\n" / AUV_stern_lateral ) ; 

) 



/* convert to signed sqrt to account for volts-to- thrust relationship 



AUV_bow_ve rtical 
AUV_stern_vert ical 



2.0 

2.0 



sqrt( 6 . 0 ) * 
* sqrt 
sqrt ( 6 . 0 ) - 



sign 
( f abs 
sign 



(AUV_bow_vertical ) 
(AUV_bow_vertical ) ) ; 
(AUV_stern_vertical ) 



*/ 
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AUV_bow_latera 1 

t r- r r._ lateral 



* sqrt (fabs 
2.0 * sqrt (6.0) * sign 

* sqrt (fabs 
2.0 * sqrt (6.0) * sign 

* sqrt (fabs 



(AUV_stern_vertical ) ) ; 
(AUV_bow_l ateral ) 

(AUV_bow_l ateral ) ) ; 

(AUV_stern_l ateral ) 
(AUV_stern_lateral ) ) ; 



if 

{ 



} 



(TRACE && DISPLAYSCREEN) 

printf (*Post-sqrt thruster 
prmtf ( "AUV_bow_vertical 
printf { "AUV_stern_vertical 
printf ( ■AUV_bow_l ateral 
printf ( *AUV_stern_l ateral 



control calculated values : \n *) ; 

= %6.3f\n", AUV_bow_vertical ) ; 

= %6.3f\n", AUV_stern_vertical ) ; 
= %6.3f\n“, AUV_bow_l ateral) ; 

= %6.3f\n", AUV_stern_l ateral); 



/* constrain thruster orders + /- 24.0 volts == 3820 rpm no-load 



C J. 37Tip ( & 
c 1 amp f & 
C-amp L 
clamp 



AUV_bow_vertical , 
AUV_st ern_vert i cal , 
A UV_bow_l ateral , 
AUV_stern_lateral , 



-24.0, 24.0, - AUV_bow_vert ical * ) ; 
-24.0, 24.0, *AUV_stern_vertical " ) ; 
-24.0, 24.0, * AUV_bow_la teral " ) ; 

-24.0, 24.0, "AUV_stern_lateral ■ ) ; 



if ( (port_rpn — command == 0.0) && ( stbd_rpm_command = = 0.0)) 
/* prevent planes chatter by zeroing them at zero speed 

; 

Oc 1 ruddei - 0.0; 

de-ta planes = 0.0; 



V 



*/ 



/* Normalization within bounds --------------------- * / 

d*rl t a_rudder = normalize/ (delta_rudder); 
dtrl t a_pl anes = normalize2 (del ta_planes) ; 

'* constrain planes & rudder orders 40.0 degrees */ 

1 : fans (del ta_ruader ■ > (40.0 /* degrees */)) 

i 

delta_rudder = (40.0) * (del ta_rudder / fabs (del ta_rudder ) ) ; 

J 

if (fabs ( del ta_pl anes j > (40.0 /* degrees */) ) 



deita_planes = (40. 0; * (del ta_planes / fabs (del ta_planes ) 



/* Send commands to rudders and planes ***********************************/ 

rudder (del ta_rudder ) ; 
planes ; del ta_pl anes ) ; 

i y Send command & get reply from sonar *********************************** / 

AUV_STlu00_bearing =0.0; /* relative bearings of sonar heads * / 

AUV_ST72S_bearing = 0.0; 



/* send telemetry to tactical level and data recording files ------*/ 

record_data (); 

/* read commands from tactical level 
if (TACTICAL == TRUE) read_paral lel_port (); 



/* update simulation clock * t " 

t = t + dt; 

f flush (stdout ) ; 
currentloopclock = clock (); 
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if ((REALTIME == TRUE) && LOCATIONLAB && 

(cur rentloopcl ock < previ ousloopclock 

+ (int ) (dt * (float) CLOCKS_PER_SEC) ) ) 



if (TRACE && DISPLAYSCREEN) 

{ 

printf ("currentloopclock = %ld, previ ousloopclock = %ld\n", 
currentloopcl ock , previousloopclock) ; 

print f ( “ timestep dt = %5.3f seconds (corresponding clock ticks = %d)\n", 
dt , ( int ) (dt * (float) CLOCKS_PER_SEC) ) ; 

printf ("Busy wait until system clock reaches simulation clock, "); 
printf ("loop duration = %5.3f\n", 

((float) currentloopclock - (float) previousloopclock) 

/ CLOCKS_PER_SEC) ; 

) 

while (currentloopclock < previousloopclock 

+ (int ) (dt * (float) CLOCKS_PER_SEC) ) 

( 

currentloopclock = clock (); /* %%%%% busy wait %%%%% */ 

) 

if (TRACE && DISPLAYSCREEN) 

{ 

printf ("Busy wait complete, loop+wait duration = %5.3f, ", 

((float) currentloopclock - (float) previousloopclock) 

/ CLOCKS_PER_SEC) ; 

printf ("current clock () = %ld\n", currentloopclock); 



else if ; (REALTIME == FALSE) && LOCATIONLAB && DISPLAYSCREEN && TRACE) 

printf ("No busy wait, loop duration = %5.3f, ", 

( (float) currentloopclock - (float) previousloopclock) 
/ CLOCKS_PER_SEC) ; 

printf ("current clock () = %ld\n", currentloopclock); 

} 

previcusloopcl ock = clock (); 



’ estimate X and Y by dead reckoning 



V 



x ~ x - (speed * dt * cos (radians (psi))); 
v = y * (speed T dt * sin (radians (psi))); 



i 

else 



{ 



) 



ifecf (auvscriptf ile) && (t >= time_next_command) ) /* all done */ 

if (TRUE && DISPLAYSCREEN) printf ("end_test set TRUE\n " ) ; 
ena_test = TRUE; 

if (t >= time_next_command) /* scriptfile not yet closed, read more 
if (TRUE && DISPLAYSCREEN) 

printf ("\n[read more from parse_mission_script_commands]\n"); 
parse_mission_script_commands (); /* get next script orders read 



*/ 



* / 



if (TRACE && DISPLAYSCREEN) 

printf ("[finish closed_loop_control_module ()]\n B ); 



return; 

} /* end closed_loop_control_module () */ 
void get_control_constants () 

/* get data front file at program start */ 

if (TRACE && DISPLAYSCREEN) 
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printf {"[start get_cont rol_constants ()]\n“); 

if ' ENTER CONTROL CONSTANTS ) 

( 

printf { " Input start_dwel I \ n" ) ; 
scanf ( "%d" , &start_dwell ) ; 

/* note %F required by OS-9, accepted by SGI */ 
printf (" Input k_psi , k_r, k_v\n"); 
scanf ("%F %F %F", &k_psi , &k_r, &k_v) ; 

print f {" Input k_z, k_w, k_theta, and k_q\n'); 
scanf ("%F %F %F %F", &k_z, &k_w, &k_theta, &k_q) ; 

printf ( " Input k_thruster_psi # k_thruster_r\n" ) ; 
scanf riF %F", &k_thruster_psi , &k_thruster_r) ; 

printf { " Input k_thruster_rotate\n " ) ; 
scanf - "%F" , &k_thruster_rotate ) ; 

printf { " Input k_thruster_z , k_thruster_w\n" ) ; 
scanf * o F %F " , &k__thruster_z , &k_thruster_w); 

printf { " Input k_propel ler_hover , k_surge_hover \n " ) ; 
scanf ("%F %F", &k_propel ler_hover , &k_surge_hover) ; 

printf [ " Input k_thruster_hover , k_sway_hover\n " ) ; 
scanf f"%F %F", & k_thruster_hover , 

&k_sway_hcver } ; 

print f i " Input speed_limit from 1 to 2 feet /sec \n*); 
scanf ( "%F M , &speed_lim: t ) ; 

print f (" Input rpm from +-200.0 to +-700 . 0\n* ) ; 
scanf ( '%F M , &rpm/ ; 

else /* use default initialization values 



if {TRACE && D I S PLAY SCREEN ) 

printf ("{using default control constant values] \n"); 



star:_dwel 1 = 


1; 


/• delay 


time in 


seconds */ 


k_psi 


1.00; 


/* degrees rudder 


per degree of course error */ 


k r 


2.00; 


/* degrees rudder 


per degree/sec yaw rate */ 


k_v 


C .00; 


/* needed ?? */ 




k z 


15.0; 


/* degrees planes 


per foot of depth error V 


k_w 


2.0; 










k_theta = 


4.0; 










k_q 


1.0; 










rpm 


= 400.0; 








k_thruster_psi 


= 


0.6; 


/* 


volts per 


1 degree course error *f 


k_thruster_r 


= 


5.0; 








k_thruster — rotate = 


2.25; 


/* 


(24V) / '2=-> 


2 # = 16.0 deg/sec empirical*/ 








/* 


k__thrus ter_rotate= (24V / 16 deg/sec) ^2*/ 


k thruster_lateral= 


o 

CD 


/* 


24 V = 2 


# = 0.5 ft/sec empirically */ 








/* 


note voltage follows a square law */ 


k_thruster_z 




50.0; 


/* 


guesses * 


/ 


k_thruster_w 


= 


50.0; 








k_propeller_hover = 200.0; 


/* 


200 rpm 


per one foot error */ 


k_surge_hover 


= 6000.0; 


/* 


60 rpm 


per 0.01 foot/sec surge */ 
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/* this value is high to reduce sternway */ 



k_thruster_hover = 4.0; 

k_sw ay _n o v e r = 4 C . 0 ; 



speeo_l imi t = 2.0; /* 1.0 to 2.0 ft /sec */ 

if (TRACE DISPLAYSCREEN) 

( 

print f (“[k_psi = %5.2f, k_r = %5.2f, k_v = %5.2f, k_z = %5.2f, • , 
k_psi, k_r, k_v, k_z) ; 

printf ( • k_w = %5.2f, k_theta = %5.2f, k_q = %5.2f]\n*, 
k_w, k_theta, k_q) ; 

printf ( • [k_thruster_psi = %5.2f, k_thruster_r = %5.2f, ", 
k_thruster_psi , k_thruster_r) ; 

printf ( * [k_thruster_rotate = %5.2f, ", 
k_thruster_rotate ) ; 

printf ( ■ k_thruster_z = %5.2f, k_thruster_w = %5.2f]\n", 
k_thruster_z , k_thruster_w) ; 

printf ( " k_propel ler_hover = %5.2f, k_surge_hover = %5.2f]\n" / 

k_propel ler_hcver , k_surge_hover ) ; 

print! : " k_thruster_hover = %5.2f, k_sway_hover = %5.2f]\n", 

k_thruster_hover , k_sway_ hover ) ; 

if ■ controiconstantsfile = fopen (CONTROLCONSTANTSFILENAME , *w " ) ) == NULL) 

printf ( “AUV execution: unable to open output file %s for writing. 

CONTROLCONSTANTSFILENAME) ; 

printf 

Check ownership permissions in current di rectory . \n * ) ; 

Flint: : “Exit. \n" ) ; 

tr'X 1 t ( - 2 ; ; 



if . TRACE DISPLAYSCREEN) 

printf ' B ( cont rolconstantsf ile %s open, pointer = %x]\n", 
CONTROLCONSTANTSFILENAME, controiconstantsfile) ; 



f printf 
f printf 
f printf 
f printf 



( controiconstantsfile, 

" \n\n" ) ; 

( controiconstantsfile, 

■ AUV execution level control algorithm coefficients \n“ ) ; 

( controiconstantsfile, 

“ \n\n\n " ) ; 

(cont rolconstantsf ile. 



• k_psi k_r k_v 

( controiconstantsfile, 


k_z 


k_w 


k_theta 


k_q 


\n\n* ) ; 


- %5.2f %5 . 2f %5 . 2 f 


%5.2f 


%5 . 2 f 


%5 . 2 f 


%5.2f 


\n\n\n\n* 


k_psi, k_r, k_v, 


k_z, 


k_w, 


k_theta, 


k_q 


) ; 



fprintf (controlconstantsf ile, 
• k_thruster_psi 
fprintf (controiconstantsfile, 
%S . 2 f 

k_thruster_psi , 



k_thruster_r 

%5.2f 

k_thruster_r , 



k_thruster_rotate\n\n* ) ; 

%S.2f \n\n\n\n", 
k_thruster_rotate) ; 



fprintf 

fprintf 



( controlconstantsf ile , 
" k_thruster_z 
(controlconstantsf ile, 
• %5.2f 

k_thruster_z , 



k_thruster_w \n\n"); 

%5 . 2 f \n\n\n\n*, 
k_thruster_w) ; 



fprintf (controiconstantsfile, 

" k_ propel ler_hover k_surge_hover \n\n"); 
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f 



f (cone rclconstantsf i le, 

% 5 . 2 f 

k_propel ler_ hover , 



%5 . 2 f \n\n\n\n " , 
k_surge_hover) ; 



fprintf ( cont rolconstantsf i le, 
" k_thruster_hover 
fprintf (cont rolconstantsf i le , 
%5 . 2 f 

k_thruster_hover , 



k_sway_hover \n\n" ) ; 

%5.2f \n\n\n\n", 

k_sway_hover ) ; 



f flush (controlconstantsf ile) ; 
fclose (control constant sf lie); 



if 'TRACE && DISPLAYSCREEN) 

piintf l “[finish get_cont rol_cons tants ( ) ) \n " ) ; 



return; 



* /* end get_control_constants () */ 



doutle depth f : 

/ 

in: val = 0; 

double new_z = 0.0; 
double z_or f set = 0.G; 

if TRACE a I I SPLAY SCREEN 1 } printf (-(start depth ()]\n“); 
if , LOCATION LAE && DEADRECKON) 
z = z_corrjrand; 

) 

i f ( NOT_Y ET_R EIMPLEMENTED) 

z_cf f set = 0.0; 

val = add' ' 0 , 0; ; 

new_z = 0.002237 * (val - z_val0) * z_ offset; /* new_z (ft) */ 

e.s»r nev. z = z ; 

if (TRACE && DISPLAYSCREEN) 

printf (“[finish depth (), returns %5.3f]\n“, new_z) ; 

r^t jrn ( new_z i ; 

} / * end depth {) */ 



double calcul ate_psi () 
{ 



unsigned short psijait; 

int psi_bi t_int , psi_bi t_ol d_in t , del ta_psi_bi t ; 
double angle, tpi ; 
double pi = 3.1415927; 

if (TRACE && DISPLAYSCREEN) printf (-[start cal culate_psi ()]\n B ); 

if (LOCATIONLAB && DEADRECKON) 

{ 

psi = psi_coinmand; 

} 



NCT_Y ET_P E I M PLEMENTED ) 



* per: nc^d: re be redone: */ 

psi_bit = Read_PortAB (( struct MFI_PIA *) MFI_BASE) ; 

psi_bit &= 0x3FFF; 
psi_bit_int = psi_bit; 
psi_bi t_old_int = psi_bit_old; 

del ta_psi_bi t = psi_bit_int - psi_bit_old_int; 
ps:_bit_old = psi_bit; 

1 f (abs (del ta_psi_bi t ) > 10000) 
t 

wrap_count = wrap_count - del ta_psi_bi t /abs (del ta_psi_bi t ) ; 

) 

tpi = 2 . 0 * pi * wrap_count; 
angle = heading!) - dg_offset 4 tpi; 
angle = degrees (angle); 
else angle - psi; 

-f •' TRACE && DISPLAYSCREEN } 

print f ("(finish cal cul ate_ psi () returns %5.3f]\n", angle); 
return (normalize (angle)); 

* end calcuiat e_p s i ( ) */ 



»***»»▼'•*****»■***»»»»*»*******■****»■»■****★**■*■******'»*•***»*****»■************* 



► u z e * c 



ic data 









in: indtrx; 

:nt save_trace; 



save_trace - TRACE; 



if ‘TRACE && DISPLAYSCREEN) printf ("[start zero_gy ro_data ()]\n“); 



pi tch_0 

rcll_0 

rol l_rate_0 

pi tch_rate_0 

yaw_rate_0 

z_val0 

dg_of f set 



= a del ( 6 ) ; 

= add (7) ; 

= add ( 9 ) ; 

= add (8) ; 

= add (10); 

= adc2 (0, 0) ; 
= heading ( ) ; 



if (TRACE && DISPLAYSCREEN) 

printf ("[device averaging for 2 seconds... ) \n 
for (index=0 ; index<99; ++ index) 



pi tch_0 
rol 1_0 
roll_rate_0 
pi tch_rate_0 
yaw_rate_0 
z_val0 



add (6) ; 
add (7) ; 
add (9) ; 
add (8) ; 
add (10) ; 
adc2 (0,0) ; 



TRACE 
dg_of f set 
TRACE 

t s 1 eep ( 5 ) ; 



FALSE; 

heading!); /* this is verbose if TRACEd */ 
save.trace; 

/* 256ths of a second */ 
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) 



pi cch_C 


= pi t ch_G / 1 00 ; 




roli_0 


= rol 1_0/ 100; 




rol l_rate_ 


0 = roll_rate_0/100; 




pi tch_rate 


_0 = pit ch_r a t e_0 / 1 0 0 ; 




yaw_ra te_0 


= yaw rate 0/100; 




z_val0 


= z_val 0/ 100 ; 




dg_of f set 


= dg_of f set /100 . 0 ; 




i f \ TRAC E 


DISPLAYSCREEN) 




i 

print f 


("pitch_0 = 9 od\n", 


pitch_0) ; 


printf 


»"roll_0 = %d\n", 


roll_0 ) ; 


i : nt f 


( " rol l_rat e_0 = %d\n" , 


roll_rate_0) ; 


print f 


{ "pi tch_rate_0 = %d\n", 


pi tch_ ra te_0 ) ; 


p: :nt f 


t"yaw_rste_0 = %d\n". 


yaw_rate_0 ) ; 


print f 


(*z_val0 = %d\n", 


z_va 10); 


printf 

) 


("dg_offset = %f\n", 


dg_of f set ) ; 


if TRACE 


&& DISPLAYSCREEN) printf 


( " ( finish zero. 


re t u r n ; 







end zerc_gy ro_data () */ 






r ***** 1 



void in: tiaii ze_dacs () 



/* Initialize all dac channels to zero */ 



if (TRACE && DISPLAYSCREEN) printf {“(start ini tialize_dacs ( ) ] \n * ) ; 



if 

i 



f N0T_YE7_RE IMPLEMENTED j 



cont rol_sur f ace 
cent rcl_sur face 
cont rol_surf ace 
cent rol_surf ace 
cont rol_surf ace 
cert rol_surf ace 
cent rol_surf ace 
cont rcl_sur face 



( FRONT_RUD_TOF ,0.0); 
(FR0N7_RUD_B0T, 0.0); 
(FRONT_FL_RIGHT, 0.0) ; 
(FRONT_PL_LEFT,0 .0) ; 

{ REAR_RUL_TOP ,0.0) ; 
(REAR_RUD_BOT,0.G) ; 
(REAR_PL_RIGHT, 0 .0 j ; 
(REAR_PL_LEFT, 0.0) ; 



rr.a : r_mct ors_ef f 



if (TRACE && DISPLAYSCREEN) printf ("(finish i ni t i al i ze_dacs { ) ] \n * ) ; 
return ; 

) /* end ini tialize_dacs */ 



^*****************************t*«*t**«**»*****»t«**»*«**t******«****t**4*t***t*^ 

void ini tiaii ze_adcs () 

( 

if (TRACE && DISPLAYSCREEN) printf ("(start ini tiaii ze_adcs ( ) ) \n • ) ; 

#if defined (sgi) 

#else 

/* Initialize MFI channels: 0 = input port, 1 = output port */ 

Init PortA ((struct MFI_PIA *) MFI_BASE, MFI_INPUT_PORT) ; 

Ini t_PortB ((struct MFI_PIA *) MFI_BASE, MFI_INPUT_PORT) ; 

#endi f 
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-f (TRACE && DISPLAYSCREEN) printf ("(finish ini ti al ize_adcs ( ) 1 \n ■ ) ; 
return ; 

i , T end ini t ial i ze_adcs *1 



«»*»*»•»*************»*»********* + ******★******************★*★***************** j 



void control_surf ace (surface, angle) 

/* This function sends the desired ANGLE to the specified control SURFACE 
The angle is first normalized to (-45 to 45), then correction is applied 
for the non-linearity in the servo control module */ 

int surface; 

double angle; 

( 

in: vclt; 
double a,b,c,d; 

if (FALSE && DISPLAYSCREEN) printf (“(start control_sur f ace ( ) J \n " ) ; 



if ' N CT_Y ET_R E I M PLEMENTED ) 



a = : . 2 4 S 7 e - 4 ; 

= -2 . 9 0 c " e - 2 ; 

d = 500.6576; 

ancle = angle*57 .295779; /* Convert RADIANS to DEGREES */ 

if Mangle < -22.92) II (angle > 22.92)) 

/* Plane saturated set to *- 45 */ 
angle = 22 . 92 * angle/ fabs ( angle ; ; 



vclt = a*pow (angle , 3 . ) + b*pow (angle, 2 . ) ♦ c*angle + d; 
dac2b ( vol t , surface) ; 



) 

if (FALSE && DISPLAYSCREEN) printf (• (finish control_surf ace ()]\n“); 



return ; 

/ /* control_surf ace */ 



^»***»**»»*»***»********»**»*»***»*»*»»*»******»************************^***#**y 



void rudder (angle) 

/* Send angular deflection (RADIANS) to rudders. 

Convention ( + ) angle left turn, (-) angle right turn */ 



I 



double angle; 

if (TRACE && DISPLAYSCREEN) printf (“[start rudder ()]\n“); 



control_surf ace 
cont rol_surf ace 
con trol_sur face 
control_surf ace 



(FRONT_RUD_TOP, -angle) ; 
( FRONT_RUD_BOT , ang 1 e ) ; 
(REAR_RUD_TOP, angle) ; 
(REAR_RUD_BOT, -angle) ; 
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if TRACE DISPLAYSCREEN - printf ("[finish rudder ())\n"J; 
return; 

} / * rudder */ 

/*•*** *** ******************** ********************************** *».*♦»*.*./ 

vcid planes (angle) 

/* Send angular deflection (RADIANS) to bow and stern planes. 

Convention f-o angle dive, ( - ) angle rise */ 

double angle; 

{ 

if (TRACE && DISPLAYSCREEN) printf ("[start planes ( ) ) \n" ) ; 

con: rol_surf ace (FR0NT_PL_R1GHT, anal e) ; 
con: rol_surface (FRONT_PL_LEFT, -angle) ; 
cont rcl_surf ace (REAR_FL_PIGHT, -angle) ; 
cont rcl_surf ac^ ( REAR_PL_LEFT , angle) ; 

if (TRACE && DISPLAYSCREEN; printf ("[ f ini sh planes ( ) ) \n“ ) ; 
return; 

j i * end planes ( ) * / 

vcid msir_motcrs_cf f () /* Turn off both main motors */ 

t 

-f TRACE txb 1 1 SPLAYSCREEN , printf ("[start mai n_motors_of f ( ) ) \ n " ) ; 

l f iN07_YET_RE:MFLEMENTED} 

dacl (512, SUPPLY) ; 
dad f E 1 2 , R I GHT_MOTOP ) ; 
dacl '512, LEFT_MOTOR ) ; 

i: TRACE *L 2 1 SPLAYSCREEN j printf {"[finish main_motors_of f ()]\n“); 

return; 

} /* end m.a in_motors_of f {) */ 

'* * *************** ****’********•*******************************/ 



void alive (interval, local_start_dwel 1 ) 
unsigned int interval; 



{ 



int local_start_dwell ; 

unsigned int i interval , jinterval ; 
double test_delta; 

if (TRACE && DISPLAYSCREEN) printf ("[start 



i f (NOT_YET_RE IMPLEMENTED) 
( 



alive ( ) J \n" ) ; 



local_start_dwel 1 = local_start_dwel 1* 100; 
interval = interval* 100 ; 
iinterval = local_start_dwell/interval; 
jinterval = 0; 

test_delta = .4; /* Deflect 22.5 degrees */ 
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whi le ( 3 i nterval < iinterval) 
f 

cone : ol_sur face ( FRONT_RUD_TOP, test_delta) ; 
t sleep ( interval ) ; /* 256ths of a second */ 

test_delta = -test_delta; 

^interval = jinterval * 1 ; 

} 

tsieep(200); /* 2S6ths of a second */ 

) 

if (TRACE && DISPLAYSCREEN) printf ("(finish alive ( ) ] \n " ) ; 
return ; 



/*»»»»»»*■*■*»*»»****■*■*»*****■****■***■»****■»■*■**■**■***■**■**■**■***■*■**♦★***»*****★★**★★★* j 

void i*ecord_da ta_on () 

if (TRACE && DISPLAYSCREEN) printf ("[start record_data_on ( ) ] \n" ) ; 

/* Open files for writing */ 

If • (auvdatafile = fopen ( AUVDATAFILENAME, "w" ) ) == NULL) 

printf ' "AUV execution: unable to open output file %s for writing. ", 

AUVDATAFILENAME) ; 

printf 

(“ ChecK ownership permissions in current directory . \n" ) ; 

printf ( " Ex i t . \ n “ ) ; 
exit (- 1 . ; 

/ 

if '.TRACE && DISPLAYSCREEN) 

printf ( " ! auvdataf ile %s open, pointer = %x]\n", 

AUVDATAFILENAME, auvdatafile); 

if i (auvtextfile = fopen (AUVTEXTFILENAME, "w" ) ) == NULL) 

/ 

printf ("AUV execution: unable to open output file %s for writing. 

AUVTEXTFILENAME) ; 

printf 

{" Check ownership permissions in current di rectory . \n ") ; 

printf ( "Exi t . \n " ) ; 
exit (- 1 ); 

) 

if f TRACE && DISPLAYSCREEN) 

printf (“(auvtextfile %s open, pointer = %x]\n“, 

AUVTEXTFILENAME, auvtextfile); 

fprintf (auvtextfile, "* auvtextfile %s shows state ", AUVTEXTFILENAME); 
fprintf (auvtextfile, "vector variables at one second intervals . \n\n ") ; 

if (LOOPFOREVER) 

fprintf (auvtextfile, "# Mission replication #%d\n", 

replication_count ) ; 

/* testing code from wr 2 tl.c, not currently in use */ 

/* serial. d is a telemetry test file to check connectivity */ 

/* if ( (serialtestf ile = fopen ("serial. d", "r")) <= 0 ) 

( 

printf ("AUV execution: record_data_on : can't open test file serial .d\n" ) ; 

printf ( "Exit . \n" ) ; 
exi t ( - 1 ) ; 

} 

*/ 

if (TRACE && DISPLAYSCREEN) printf ("[finish record_da ta_on ()]\n"); 
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return ; 



y r '»»»»»**»*«»»***»»*»************»'tir***»'**»*******************'t***************** 

void record_data_cf f () 



if (TRACE ii DISPLAYSCREEN) print f ("(start record_data_of f ()]\n“); 

if (TRACE ii DISPLAYSCREEN) 

( 

printf ("[flushing and closing auvdatafile %s %x]\n’ / 

AUVDATAFI LENAME , auvdatafile) 

f flush (stdout ) ; 

} 

if (auvdatafile != NULL) 

{ 

if (TRACE ii DISPLAYSCREEN) printf ("[auvdatafile f lushed] \n" ) ; 
f flush (Stdout); 
f flush (auvdatafile); 
fclose (auvdatafile); 

if (TRACE ii DISPLAYSCREEN) printf ("[auvdatafile closed] \n"); 
f flush (stdout); 

) 

else if (TRACE ii DI SPLAYSCREEN) printf ("[auvdatafile was not open!!]\n") 

if (TRACE && DISPLAYSCREEN) 

printf "[flushing and closinq auvtextfile %s %x]\n", 

AUVTEXTFI LENAME, auvtextfile) 

f flush \ Stdout i ; 

} 

if (auvtextfile != NULL) 

/ 

if 'TRACE ii DISPLAYSCREEN; printf ("[auvtextfile f lushed ] \n ") ; 
f flush 'stdout / ; 
f flush (auvtextfile) ; 
fclcse (auvtextfile); 

if (TRACE ii DISPLAYSCREEN; printf (’[auvtextfile closed] \n " ) ; 
fflusr, i stdout,; 
i 

else if (TRACE && DISPLAYSCREEN' printf ("[auvtextfile was not open!!]\n") 

/ * fclose { serial testfile; ; /* serial port test file */ 

if (TRACE ii DISPLAYSCREEN) 

\ 

printf ("[finish recora_data_of f ()]\n"); 
f flush (stdout); 

} 

return ; 



) 

/*ttt**»****tt*********t*****************tt»tt*»****t»**t**t***tt*i***t****t*t* 



void record_data () 

{ 

/* temporary hold varia 

dou bl e AUV_t ime_temp , 
AUV_x_temp , 
AUV_phi_temp, 
AUV_u_temp, 
AUV_p_temp, 
AUV_x_dot_t emp , 



es V 



AUV_y_temp, 
AUV_theta_temp, 
AUV_v_temp, 
AUV_q_temp, 
AUV_y_dot_temp , 



AUV_z_temp, 
AUV_psi_temp, 
AUV_w_temp, 
AUV_r_t emp , 
AUV_z_dot_temp, 
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